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1  Introduction 

Real-time  tracking  of  the  orientation  or  attitude  of  rigid  bodies  has  wide 
applications  in  robotics  [1],  helicopters  [2],  tele-operation,  augmented  reality, 
and  virtual  reality  [3].  Limb  segment  orientation  can  be  estimated  through  the 
attachment  of  an  inertial/magnetic  sensor  module  to  each  segment  as  depicted  in 
Figure  1.  Given  the  length  of  each  of  the  segments,  their  estimated  orientation 
based  on  sensor  module  data,  and  their  arrangement  relative  to  one  another,  the 
posture  of  the  body  can  be  determined.  This  method  of  orientation  and  posture 
estimation  is  desirable  since  it  is  not  dependent  on  any  artificially  generated 
reference  signal  and  does  not  suffer  from  any  line  of  sight 
restrictions  [4]. 

Inertial/magnetic  sensor  modules  and  their  associated  data  filtering 
algorithms  are  designed  to  be  capable  of  estimating  three  degrees  of  orientation 
over  a  wide  area  in  a  variety  of  unprepared  tracking  environments.  The  sensor 
modules  commonly  contain  three  linear  accelerometers  and  three 
magnetometers.  The  accelerometers  are  orthogonally  mounted  in  a  triad  as  are 
the  magnetometers.  Sensor  modules  designed  for  more  dynamic  applications 
may  also  contain  three  orthogonally  mounted  angular  rate  sensors  for  use  as  a 
high  frequency  data  source.  Each  of  the  triads  is  mounted  such  that  there  is  an 
individual  sensor  aligned  with  one  of  the  principle  axes  of  the  coordinate  frame 
of  the  sensor  module.  Thus  the  total  number  of  sensors  contained  in  modules 
designed  to  provide  data  for  estimating  orientation  in  dynamic  applications  is 
commonly  nine. 

In  orientation  estimation  algorithms  designed  to  process  inertial/magnetic 
sensor  data,  accelerometers  are  used  to  measure  the  gravity  vector  relative  to  the 
coordinate  frame  of  the  sensor  module.  Accelerometers  allow  accurate 
determination  of  pitch  and  roll,  but  can  not  be  used  to  sense  rotations  about  the 
gravity  or  vertical  axis.  Magnetometers  are  thus  commonly  used  to  measure 
azimuth  or  rotation  in  the  horizontal  plane  relative  to  a  “fixed”  reference.  The 
data  from  the  incorporated  sensors  is  normally  fused  using  a  Kalman  or 
complementary  filtering  algorithm.  Foxlin  et  al.  describes  two  commercial 
sensor  modules  containing  accelerometers,  magnetometers,  and  angular  rate 
sensors  designed  for  head  tracking  applications  [5], [6].  Sensor  fusion  is  performed  using  a  complementary  separate-bias 
Kalman  filter.  Bachman  et  al.  propose  a  quaternion-based  complementary  filter  for  human  body  tracking  [3],  [7].  The  filter 
is  able  to  track  through  all  orientations  without  singularities  and  continuously  correct  for  bias  and  drift  errors  associated 
with  low  cost  angular  rate  sensors  without  a  need  for  stationary  periods.  Gallagher  et  al.  presents  a  simpler  complementary 
filter  algorithm  with  lower  computational  complexity  in  [8].  Luinge  describes  a  Kalman  filter  designed  for  human  body 
tracking  applications  in  [9].  The  primary  difference  between  the  work  presented  in  this  paper  and  that  of  Luinge  is  that 
Luinge  does  not  use  magnetometers.  In  the  absence  of  magnetometers,  drift  about  the  vertical  axis  is  reduced  by  limiting 
body  segment  orientation  using  a  kinematic  human  body  model.  The  kinematic  model  incorporates  biomechanical 
constraints  on  the  joints.  This  method  allows  calculation  of  accurate  relative  orientation  between  adjacent  segments.  The 
proposed  Kalman  filter  is  useful  for  long  periods  of  measurement  if  only  inclination  is  required.  In  [10],  Zhu  and  Zhou 
describe  a  linear  Kalman  filter  algorithm  designed  to  smooth  accelerometer  and  magnetometer  readings.  Rather  than 


Figure  1:  Prototype  body  tracking 
system  based  on  inertial/magnetic 
sensors  modules. 
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estimating  individual  limb  segment  orientations  relative  to  a  fixed  reference  frame,  the  system  determines  joint  angles  in 
axis/angle  form  using  data  from  the  two  sensor  modules  mounted  on  the  two  segments  adjacent  to  the  joint.  Kraft  describes 
an  “unscented,”  quaternion-based  Kalman  filter  for  real-time  estimation  of  a  rigid  body  orientation  [11].  Simulation  results 
demonstrate  the  general  validity  of  the  described  filter.  Yan  and  Yuan  describe  a  single  frame  orientation  tracking 
algorithm  that  uses  low  cost  sensor  modules  to  take  two  axis  measurements  of  gravity  and  the  local  magnetic  field  [12]. 
Elevation,  roll  and  azimuth  angles  are  sequentially  calculated  and  the  method  is  limited  to  orientation  tracking  within  a 
hemisphere.  In  [13],  Gebre-Egziabher  et  al.  describe  another  single  frame  attitude  determination  algorithm  for  aircraft 
applications.  The  algorithm  is  based  on  the  QUEST  (QUaternion  ESTimator)  algorithm  [14]  which  was  originally  designed 
to  determine  spacecraft  attitude  given  a  set  of  3-dimensional  known  reference  vectors  and  their  corresponding  observation 
or  measurement  vectors.  In  the  case  of  [13],  the  local  magnetic  field  and  gravity  vectors  are  used  as  reference  vectors. 

In  the  above  studies  with  the  exception  of  the  work  by  Luinge  [9],  both  the  gravity  and  local  magnetic  field  vectors 
are  treated  as  fixed  references.  In  the  case  of  the  gravity  vector,  the  assumption  that  it  is  fixed  leads  to  no  difficulties  since 
this  vector  does  in  fact  point  straight  down  in  any  inertial  frame  located  on  or  near  the  surface  of  the  earth.  Making  the 
same  assumption  regarding  the  local  magnetic  field  vector  can  however  lead  to  problems.  In  a  typical  room  setting,  the 
direction  as  well  as  the  magnitude  of  the  local  magnetic  field  vector  can  be  expected  to  vary  due  to  the  presence  of  ferrous 
objects  or  electrical  appliances.  Relative  weighting  can  be  used  to  reduce  the  weight  applied  to  magnetometer  data  in 
comparison  to  other  sensor  information.  However,  slow  drift  about  the  vertical  axis  in  the  presence  of  a  sustained  change  in 
the  direction  of  the  magnetic  field  vector  will  still  occur.  Reducing  the  weight  given  to  magnetic  data  does,  however,  make 
it  possible  to  reduce  orientation  errors  resulting  from  transients  in  the  local  magnetic  field.  Such  weighting  techniques 
allowing  manual  adjustment  of  magnetometer  gains  are  described  in  [3],  [5],  and  [8]. 

This  paper  describes  several  experiments  designed  to  examine  small  scale  magnetic  interference  caused  by  typical 
objects  and  how  this  interference  can  be  expected  to  affect  the  accuracy  of  orientation  estimates  produced  using  data  from 
inertial/magnetic  sensor  modules.  The  results  provide  insight  into  the  limitations  of  inertial/magnetic  sensor  module 
orientation  tracking.  They  indicate  that  while  errors  due  to  local  variations  in  a  common  room  environment  caused  by 
individual  objects  can  be  significant,  in  most  cases  they  can  be  avoided  by  maintaining  a  separation  of  approximately  one 
meter  from  the  source  of  interference.  The  interference  caused  by  combined  sources  in  a  noisy  indoor  environment  can 
however  be  quite  complex.  The  results  also  indicate  that  inertial/magnetic  sensor  modules  can  be  used  to  track  link 
orientation  of  a  mechanical  arm  relative  to  an  Earth  fixed  reference  frame. 

The  remainder  of  this  paper  provides  a  brief  background  and  then  presents  a  series  of  experiments.  Section  2 
describes  the  sensors  used  in  the  experiments,  gives  a  brief  overview  of  magnetic  fields,  and  magnetometer  calibration 
methods  that  can  be  used  to  deal  with  various  types  of  magnetic  interference.  Section  3  presents  data  from  four  different 
experiments  in  which  inertial/magnetic  sensor  modules  are  subjected  to  controlled  changes  in  the  magnetic  field,  exposed 
at  varying  distances  to  sources  of  magnetic  interference,  and  used  to  track  a  robotic  arm.  The  final  section  of  the  paper 
discusses  the  implications  of  the  experimental  results  for  the  use  of  inertial/magnetic  sensor  modules  and  provides  a 
summary. 

2  Background 

The  following  paragraphs  go  deeper  into  the  theory  of  orientation  estimation  algorithms  designed  for 
inertial/magnetic  sensor  modules  and  briefly  describe  three  types  of  sensor  modules.  Specifically,  the  modules  discussed 
are  the  InterSense  InertiaCube,  the  MicroStrain  3DM-G,  and  the  MARG  III.  The  MARG  III  was  designed  by  the  authors 
and  manufactured  by  McKinney  Technology.  Basic  background  on  the  ambient  magnetic  field  of  the  Earth  and  how  it  is 
distorted  by  ferrous  objects  and  electrically  powered  devices  is  then  provided.  Methods  of  calibrating  magnetic  field 
variations  are  then  discussed. 

2.1  Inertial/Magnetic  Sensor  Modules 

Inertial/magnetic  sensor  modules  have  be  been  fabricated  by  both  industry  and  university  research  laboratories. 

Filtering  algorithms  designed  for  these  sensor  modules  are  based  on  inertial  and  magnetic  quantities  directly  related  to  the 

motion  and  orientation  of  a  sensor  module.  Algorithms  designed  for  use  with  inertial/magnetic  sensor  modules  produce 
accurate  orientation  estimates  by  taking  advantage  of  the  complementary  nature  of  the  sensed  quantities  in  order  to 
determine  orientation. 

For  a  static  or  slow  moving  rigid-body,  accelerometer  triad  output  can  normally  be  averaged  (or  low  pass  filtered)  for 
a  short  period  of  time  in  order  to  measure  the  components  of  the  gravity  vector  in  the  sensor  coordinate  frame. 
Determination  of  the  relationship  of  the  measurement  in  the  sensor  coordinate  frame  to  the  gravity  vector  in  Earth 
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coordinate  frame  allows  estimation  of  orientation  relative  to  the  horizontal  plane.  However,  in  the  event  that  the  sensor 
module  is  rotated  about  the  vertical  axis,  the  projection  of  the  gravity  vector  on  each  of  the  principle  axes  of  the 
accelerometer  will  not  change.  Since  the  accelerometer  triad  can  not  be  used  to  sense  a  rotation  about  the  vertical  axis,  an 
orthogonally  mounted  triad  of  magnetometers  can  be  used  to  measure  the  local  magnetic  field  vector  in  body  coordinates 
and  allow  determination  of  orientation  relative  to  the  vertical.  Thus,  combining  magnetometer  data  with  accelerometer  data 
provides  a  complete  method  for  estimating  the  orientation. 

Alternatively,  assuming  the  initial  orientation  of  the  body  is  known,  integration  of  the  output  of  a  triad  of  orthogonally 
mounted  angular  rate  sensors  provides  another  method  of  estimating  orientation.  If  the  rate  sensors  are  susceptible  to  noise 
or  bias  effects,  as  is  the  case  for  the  small  low  cost  sensors  used  in  inertial/magnetic  sensor  modules,  these  estimates 
become  useless  after  a  short  period.  To  avoid  lag  or  overshoot  in  dynamic  applications,  many  inertial/magnetic  sensor 
filtering  algorithms  combine  high  frequency  angular  rate  sensor  data  with  low-frequency  accelerometer  and  magnetometer 
data  in  a  complementary  manner  to  produce  continuously  accurate  orientation  estimates  in  real-time. 

Based  on  the  work  of  Foxlin,  InterSense  Inc.  developed  and  marketed  a  sensor  module  called  the  InertiaCube2.  The 
primary  application  for  this  module  is  head  tracking.  Manufacturer’s  literature  indicates  that  the  InertiaCube2  is  capable  of 
measuring  angular  rates,  linear  accelerations,  and  the  local  magnetic  field  along  three  axes.  Dimensions  for  the 
InertiaCube2  are  29  mm  x  24  mm  x  34  mm.  Orientation  estimates  are  made  by  a  proprietary  extended  Kalman  filter  [5], [6]. 
Manufacturer’s  literature  lists  an  accuracy  of  1.0  degree  and  an  update  rate  of  180  Hz. 

The  3DM-G  Gyro  Enhanced  Orientation  Sensor  also  contains  a  triad  of  orthogonally  mounted  angular  rate  sensors,  a 
triad  of  orthogonally  mounted  accelerometers,  and  a  triad  of  orthogonally  mounted  magnetometers.  Sensor  data  is 
processed  by  a  proprietary  filtering  algorithm  running  on  an  embedded  microcontroller.  Manufacturer’ s  literature  lists  an 
accuracy  of  +/-  5  degrees  for  arbitrary  orientations.  Unlike  the  InertiaCube2,  unsealed  as  well  as  scaled  raw  data  output  is 
available  from  this  unit.  The  update  rate  is  76.6  Hz.  Unit  dimensions  are  65  mm  x  90  mm  x  25  mm. 

The  MARG  III  sensor  module  shown  in  Figure  2  is  a  research 
prototype  developed  by  the  Modeling,  Virtual  Environments  and 
Simulation  (MOVES)  Institute  at  the  Naval  Postgraduate  School  [15]. 

Primary  sensing  components  for  this  unit  include  Tokin  CG-L43  ceramic 
rate  gyros,  Analog  Devices  ADXL202E  micromachined  accelerometers, 
and  Honeywell  HMC1051Z  and  HMC1052  one  and  two-axis 
magnetometers.  The  MARG  III  sensor  module  incorporates  a  Texas 
Instruments  MSP430F149  ultra-low-power,  16-bit  RISC  architecture 
microcontroller.  Overall,  dimensions  are  approximately  18  mm  x  30  mm  x 
25  mm.  The  sensor  module  includes  a  magnetic  set/reset  circuit  to  cancel 
magnetometer  temperature  drift  and  avoid  magnetic  saturation  effects. 

Various  complementary  and  Kalman  filters  based  on  a  quaternion 
representation  of  orientation  have  been  used  to  process  MARG  III  sensor 
data  [3],  [16].  Estimation  accuracy  has  been  measured  to  be  better  than  one 
degree. 


2.2  Magnetic  Field  Variations 

Magnetic  fields  surround  permanent  magnets  or  electrical  conductors.  They  can  be  visualized  as  a  collection  of 
magnetic  flux  lines.  Flux  lines  are  said  to  emanate  from  a  ‘north’  pole  and  return  to  a  ‘south’  pole  in  a  magnet.  Flux 
density,  or  magnetic  induction,  is  a  measure  of  the  number  of  flux  lines  passing  through  a  given  cross  sectional  area. 
Magnetic  field  strength  is  a  measure  of  force  produced  by  an  electric  current  or  a  permanent  magnet.  Magnetic  field 
strength  decreases  with  the  cube  of  the  distance  from  the  source.  While  magnetic  field  strength  and  magnetic  flux  density 
are  not  the  same,  they  are  equal  within  a  vacuum.  Magnetic  permeability  is  a  constant  of  proportionality  that  exists 
between  magnetic  induction,  and  magnetic  field  intensity.  It  can  be  viewed  as  a  measure  of  how  easily  magnetic  lines  of 
flux  will  pass  through  a  given  material.  In  the  presence  of  an  object  made  of  a  material  with  a  relatively  high  permeability, 
magnetic  field  lines  will  bend  toward  or  be  attracted  to  the  object.  Thus  distortion  can  be  expected  to  occur  near  large 
ferrous  objects  [17]. 
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Distance 

Distance 

(15cm) 

(30cm) 

Field 

Field 

(Gauss) 

(Gauss) 

Can  Opener 

1.60 

0.27 

Electric  Saw 

1.20 

0.25 

Vacuum  Cleaner 

0.75 

0.20 

Electric  Shaver 

0.65 

0.10 

Mixer 

0.61 

0.11 

Hair  Dryer 

0.50 

0.07 

Electric  Drill 

0.20 

0.03 

Portable  Heater 

0.15 

0.04 

Fluorescent  Light  Fixture 

0.13 

0.04 

Fan  (range  Hood) 

0.09 

0.03 

Television 

0.07 

0.02 

Table  1:  Common  magnetic  field  magnitudes 
in  Gauss  at  15  and  30  cm  adapted  from  [18]. 


The  direction  and  magnitude  of  ambient  magnetic  field  at  a 
given  point  is  the  vector  sum  of  all  magnetic  fields  present  at  that 
point.  The  dominate  field  in  most  cases  is  that  of  the  Earth  which 
varies  from  approximately  0.23  to  0.61  Gauss.  However,  additional 
magnetic  fields  caused  by  conductors  through  which  a  current  is 
flowing  and  magnets  also  contribute  to  the  total  field  at  a  given 
position.  All  contributing  fields  will  be  distorted  by  objects  made  of 
materials  with  a  high  magnetic  permeability. 

In  an  indoor  environment,  sources  of  magnetic  interference  are 
constantly  present  and  can  include  common  items  such  as  computer 
monitors,  fluorescent  lighting  and  powered-up  electrical  wiring  inside 
walls.  Table  1  lists  the  fields  generated  by  some  common  appliances. 
In  some  cases  the  strength  of  the  generated  field  exceeds  that  of  the 
Earth  within  a  short  distance  of  the  appliance.  If  a  magnetic  sensor  is 
placed  in  this  nearby  area  the  generated  field  can  be  expected  to  have 
an  effect  on  the  direction  and  magnitude  of  the  field  measured  by  the 
sensor.  Unless  the  field  generated  by  the  appliance  happens  to  be 
aligned  with  that  of  the  Earth,  the  reported  direction  will  not  be  that  of 
the  Earth’s  magnetic  field.  In  a  room  size  environment  such  fields 
would  constitute  local  variations  from  the  average  field  in  the  room.  It 
is  variations  of  this  type  and  their  effect  on  the  orientation  estimates 
produced  by  inertial  magnetic  sensors  with  which  this  paper  is 
concerned. 


2.3  Magnetic  Field  Calibration 

Magnetic  distortions  caused  by  ferrous  objects  that  have  a  fixed  location  and  orientation  relative  to  the  magnetometers 
being  used  to  determine  the  direction  of  the  local  magnetic  field  vector  can  be  separated  into  two  categories.  These 
categories  are  hard  iron  and  soft  iron  effects.  Hard  iron  objects  are  permanently  magnetized.  Soft  iron  objects  are  un¬ 
magnetized  unless  under  the  influence  of  a  magnetic  field. 

Hard  iron  effects  add  a  constant  offset  to  the  vector  measured  by  magnetometers  making  up  an  orthogonal  triad.  They 
can  be  compensated  for  in  the  horizontal  plane  by  rotating  the  magnetometers  together  with  the  involved  hard  iron  objects 
and  sampling  at  enough  points  in  a  circle  to  determine  the  offset  relative  to  the  horizontal  plane.  Determination  of  all 
components  of  the  offset  requires  rotation  in  more  than  one  plane.  Unlike  hard  iron  effects,  soft  iron  effects  do  not  produce 
a  constant  offset.  Soft  iron  influences  are  dependent  on  orientation  [19].  Thus  correcting  for  soft  iron  effects  often  requires 
the  construction  of  a  heading  dependent  lookup  table  [20].  Construction  of  a  three  dimensional  lookup  table  is  difficult  and 
time  consuming.  Thus,  in  a  strap-down  navigation  system,  magnetic  readings  are  usually  projected  onto  the  horizontal 
plane  using  a  tilt  sensor  before  corrections  are  made. 

In  general,  calibration  is  best  approached  by  removing  any  soft  iron  materials  and  dealing  with  hard  iron  effects 
directly.  The  magnetic  properties  of  many  materials  are  actually  in-between  those  of  soft  and  hard  iron  and  change  over 
time.  During  a  calibration  performed  at  any  given  time,  the  effects  of  such  sub-permanent  materials  will  appear  to  be 
permanent  like  hard  iron.  However,  since  the  effects  observed  are  not  truly  permanent,  calibration  procedures  must  be 
repeated  on  a  periodic  basis  [17]. 

It  should  be  emphasized  that  the  above  discussion  of  calibration  focuses  only  on  effects  caused  by  objects  that  have  a 
fixed  position  and  orientation  relative  to  a  magnetic  sensor.  In  a  tracking  application,  moving  inertial/magnetic  sensor 
modules  can  be  expected  to  constantly  change  position  and  orientation  relative  to  ferrous  objects  and  other  sources  of 
magnetic  distortion.  These  magnetic  distortions  will  not  only  change  from  position  to  position,  but  can  also  be  expected  to 
change  over  time  as  the  configuration  of  the  tracking  area  itself  changes.  The  nature  of  these  distortions  and  their  possible 
effects  on  orientation  estimation  algorithms  designed  for  inertial/magnetic  sensor  modules  is  the  primary  focus  of  this 
paper. 

3  An  Experimental  Investigation 

Inertial/magnetic  sensor  module  filtering  algorithms  are  dependent  on  sensing  the  local  magnetic  field  to  eliminate 
drift  in  the  azimuth  portion  of  orientation  estimates.  Given  that  variations  in  the  direction  and  magnitude  of  the  ambient 
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magnetic  field  can  be  expected  to  occur  as  a  result  of  the  presence  of  ferrous  materials  and  electrical  appliances  operating 
in  the  tracking  environment,  what  type  of  estimation  errors  can  be  expected  and  how  large  can  the  estimation  errors  be 
expected  to  be?  Knowing  the  answer  to  this  question  provides  insight  into  when  inertial/magnetic  sensor  modules  can  be 
expected  to  work  properly  with  minimal  estimation  error  and  what  type  of  algorithm  modifications  could  be  expected  to 
improve  performance.  The  experiments  described  below  attempt  to  answer  this  question.  In  the  first  series  of  experiments, 
several  types  of  sensor  modules  are  subjected  to  controlled  changes  in  the  direction  and  strength  of  the  sensed  magnetic 
field  in  order  to  characterize  the  resulting  orientation  estimation  errors  [21].  The  second  sets  of  experiments  involve 
exposing  a  triad  of  magnetometers  to  magnetic  fields  generated  by  various  electrical  appliances  and  ferrous  objects  in  order 
to  examine  the  magnitude  of  the  errors  and  the  range  at  which  they  occur.  In  the  last  set  of  experiments,  a  robot  arm  is 
tracked  using  inertial/magnetic  sensor  modules  and  an  optical  tracking  system. 

3.1  Errors  Caused  by  Change  in  Magnetic  Field  Direction 

In  the  first  series  of  experiments,  magnetic  field  variations  were  applied  to  the  three  types  of  sensor  modules  to 
measure  the  deviation  in  their  orientation  estimates  due  to  the  change  in  the  sensed  magnetic  field.  The  change  was 
generated  using  a  Helmholtz  coil.  The  sensors  were  placed  inside  the  coil  to  observe  how  the  orientation  estimate  would 
change  as  changes  to  the  local  direction  of  the  local  magnetic  field  were  applied.  The  three  different  sensor  modules  tested 
were  the  MARG  III,  the  MicroStrain  3DM-G,  and  the  InterSense  InertiaCube2. 

Initial  calibration  data  for  the  Helmholtz  coil  was  obtained  by  applying  different  currents  to  it  and  measuring  the 
induced  field  with  a  Hall  probe.  This  initial  data  allowed  decisions  to  be  made  regarding  how  much  current  was  necessary 
to  produce  the  desired  magnetic  inductions  to  be  applied  to  the  three  different  inertial/magnetic  sensors.  The  selected 
magnetic  field  level  was  chosen  to  be  on  the  order  of  the  Earth’s  Main  field.  The  voltage  that  was  necessary  to  reach  the 
required  magnetic  induction  was  calculated  using  linear  least  square  fit. 

During  the  experiments,  the  Helmholtz  coil  was  positioned  to  attempt  to  generate  a  magnetic  induction  that  would  be 
reversed  approximately  180°  in  azimuth  from  the  Earth’s  magnetic  field.  In  most  cases,  the  actual  measured  change  ranged 
between  160°  and  180°  due  to  imprecise  alignment  of  the  coil  relative  to  the  local  magnetic  field  vector.  Each  sensor 
module  was  placed  in  eight  different  orientations  with  in  the  field  generated  by  Helmholtz  coil.  For  each  of  the  orientations 
the  coil  was  energized  to  observe  the  type  and  magnitude  of  change  that  occurred  in  the  orientation  estimate  produced  by 
the  sensor  and  its  associated  filtering  algorithm  [21]. 

The  data  plots  from  these  experiments  show  a  period  of  measuring  the  Earth’s  ambient  magnetic  field,  followed  by  a 
period  in  which  the  Helmholtz  coil  was  energized  for  20  to  30  seconds.  Following  the  energized  period,  the  coil  was  de¬ 
energized  and  the  plots  reflect  the  return  to  sensing  only  the  ambient  field  of  the  laboratory.  The  change  in  the  direction 
and  magnitude  of  the  magnetic  field  vector  is  depicted  in  figure  3.  Energizing  the  coil  caused  the  azimuth  direction  of  the 
magnetic  field  vector  to  change  from  0°  to  180°.  There  was  no  significant  change  in  the  y  (East)  component  of  the  vector. 
Since  the  coil  was  level,  the  z  component  of  the  magnetic  field  vector  also  remained  unchanged.  Prior  to  energizing  of  the 
coil,  the  magnetic  field  vector  pointed  North  with  a  dip  angle  below  the  horizontal  of  76°.  While  the  coil  was  energized,  the 
magnetic  field  vector  pointed  South  with  a  negative  elevation  angle  of  32°.  Thus,  in  this  series  of  experiments,  not  only 
were  the  sensor  modules  exposed  to  a  full  reversal  of  the  azimuth  direction  of  the  magnetic  field  vector.  Depending  on 
their  initial  orientation  relative  to  the  magnetic  field,  the  sensor  modules  were  also  exposed  to  a  change  in  pitch,  roll,  or 
some  combination  of  the  two  totally  approximately  44°. 

For  visualization  purposes,  all  orientation  estimates  produced  by  the  sensors  are  displayed  in  Euler  angle  form.  In  the 
experiments  presented  here,  the  sensor  modules  were  oriented  in  a  North-East-Down  reference  orientation  with  the  v  axis 
of  the  module  pointing  towards  the  local  North,  the  y  axis  pointing  East  and  the  z  axis  pointing  down.  At  no  time  was  a 
sensor  actually  rotated  before,  during,  or  after  the  application  of  the  altered  magnetic  field. 
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Figure  3:  Depiction  of  total  change  in  the  direction  and  magnitude  of  the  magnetic  field  vector 

(East  is  directly  out  of  the  page). 


Figure  4  and  Figure  5  show  the  responses  for  the  MARG  III  and  MicroStrain  3DM-G  respectively  when  the  magnetic 
field  was  altered  using  the  Helmholtz  coil.  In  Figure  4,  the  MARG  III  was  placed  within  the  Hemholtz  coil  with  an  initial 
orientation  of  2°  roll,  10°  yaw,  and  3°  pitch.  Calibration  of  the  MARG  III  does  not  account  for  non-orthogonality  within 
the  magnetometer  triad.  Thus,  small  changes  and  hysteresis  can  be  seen  in  the  roll  and  pitch  estimates  and  the  yaw  estimate 
changes  by  an  amount  that  is  slightly  less  than  the  change  in  azimuth  that  occurred  in  the  direction  of  the  magnetic  field. 
The  smooth  response  of  the  MARG  III  filtering  algorithm  is  due  to  the  particular  gain  values  used  in  the  experiment.  In 
Figure  5,  the  MicroStrain  3DM-G  had  an  initial  orientation  of  2°  roll,  13°  yaw,  and  0°  pitch.  Energizing  the  coil  caused  a 
165°  change  in  the  yaw  estimate  produced  by  the  sensor  module.  This  change  was  equal  in  magnitude  to  the  measured 
change  in  azimuth.  No  significant  changes  were  observed  in  the  roll  and  pitch  estimates.  The  tuning  of  the  orientation 
estimation  algorithm  provides  an  extremely  sharp  response  to  the  change  in  the  magnetic  field  direction.  Both  the  MARG 
and  MicroStrain  sensors  responded  to  the  change  in  the  sensed  magnetic  field  by  altering  the  yaw  portion  of  their 
orientation  estimates  by  an  amount  that  was  equal  to  measured  azimuth  change  produced  by  the  Helmholtz  coil.  Neither 
showed  significant  change  in  their  roll  and  pitch  estimates  despite  the  fact  that  the  direction  of  the  magnetic  field  had 
changed  both  pitch  angle  and  azimuth  angle  as  depicted  in  Figure  3.  This  was  true  regardless  of  the  orientation  of  the 
sensor  modules  relative  to  the  coil.  This  is  significant  since  it  indicates  the  errors  due  to  magnetic  variation  are  restricted 
only  to  the  horizontal  plane.  The  estimates  of  pitch  and  roll  are  not  affected  by  changes  in  the  magnetic  field  direction  for 
the  sensors  and  algorithms  tested  [21].  This  is  in  contrast  to  some  orientation  algorithms  such  as  the  QUEST  [14],  where 
such  a  change  in  the  direction  of  the  magnetic  field  will  cause  an  error  in  both  azimuth  and  pitch. 


Pitch  Roll  Yaw  [ 


MARG  Sensor 


Figure  4:  MARG  III  sensor  response  to  180°  azimuth  change  in  the  magnetic  field  direction. 
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Figure  5:  MicroStrain  3DM-G  response  to  180°  azimuth  change  in  the  magnetic  field  direction. 

Figure  6  shows  the  response  of  the  InertiaCube2  to  the  same  magnetic  variations  as  used  in  the  experiments  depicted 
in  Figure  4  and  Figure  5.  Like  the  other  sensors  the  orientation  estimate  changes  only  in  azimuth.  However,  examination  of 
Figure  6  indicates  that  unlike  the  other  sensors,  the  estimated  orientation  produced  by  the  InertiaCube2  algorithm  changed 
by  approximately  90°  instead  of  180°. 

In  order  to  investigate  the  response  of  the  InertiaCube2  further,  additional  experiments  were  performed.  In  Figure  7, 
the  sensor  was  again  left  in  the  same  position  within  the  Helmholtz  coil.  The  coil  was  energized  for  approximately  30 
seconds.  Unlike  previous  experiments,  during  the  time  when  the  magnetic  field  was  changing  the  sensor  was  physically 
tapped.  This  caused  the  estimated  azimuth  to  proceed  through  a  change  that  is  similar  to  that  observed  with  the  other  two 
sensor  modules.  Euler  angle  azimuth  is  bounded  between  180°  and  -180°.  Though  the  change  is  expressed  as  -180°,  it  is 
equivalent  to  the  positive  180°  change  seen  with  the  other  two  sensor  modules.  The  knee  seen  in  the  trailing  edge  of  Figure 
7  is  most  likely  the  result  of  non-zero  angular  rate  readings  caused  by  tapping  of  the  sensor  module  while  the  coil  was 
being  deenergized.  These  results  indicate  that  the  filtering  algorithm  of  the  InetiaCube2  will  not  accept  changes  in  its 
orientation  estimate  without  some  accompanying  non-zero  reading  from  the  angular  rate  sensors. 


lnertiaCube2  Sensor 
Test  1  -  (+x  north)  (+z  down) 
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Figure  6:  Undisturbed  InertiaCube  2  response  to  180°  change  in  the  magnetic  field  direction. 
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lnertiaCube2  Sensor 
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Figure  7:  Disturbed  InertiaCube  2  response  to  180°  change  in  the  magnetic  field  direction. 

Based  on  the  results  of  experiments  described  above,  it  appears  that  unlike  active  magnetic  trackers  which  suffer 
estimation  errors  in  all  dimensions  due  to  magnetic  variations  [22],  variations  in  the  direction  of  the  local  magnetic  field 
only  cause  estimation  errors  in  azimuth  or  the  horizontal  plan.  The  magnitude  of  the  errors  appears  to  be  equal  to  the 
amount  of  deviation  of  the  local  magnetic  field  in  the  horizontal  plane.  No  significant  change  was  observed  in  the  pitch  and 
roll  estimates  produced  by  the  three  tested  algorithms.  These  experimental  results  indicate  that  the  dip  angle  itself  or 
changes  in  the  dip  angle  of  the  local  magnetic  field  vector  have  no  bearing  on  the  accuracy  or  amount  of  variation  seen  in 
orientation  estimates  produced  using  inertial/magnetic  sensor  module  data. 

3.2  Variations  Caused  by  Common  Objects 

To  determine  the  magnitude  of  azimuth  errors  that  can  be  expected  in  a  typical  indoor  environment,  two  types  of 
experiments  were  performed.  Initial  experiments  measured  the  magnetic  field  variation  experienced  at  varying  distances 
from  several  test  objects.  Later  experiments  measured  the  change  in  direction  of  the  magnetic  field  vector  at  several 
positions  in  a  magnetically  noisy  laboratory.  The  MARG  III  filtering  algorithm  utilizes  a  normalized  magnetic  field  vector 
of  unit  length  and  is  thus  not  affected  by  changes  in  the  length  of  the  magnetic  field  vector  [3].  Based  on  manufacturer’s 
literature,  the  algorithms  associated  with  the  InertiaCube  and  3DM-G  are  similar  in  this  regard.  Therefore,  the  experimental 
results  presented  here  concentrate  on  the  changes  in  the  direction  of  the  local  magnetic  field  and  not  changes  in  magnitude. 
The  experiments  described  above  establish  that  changes  in  the  direction  of  the  magnetic  field  orientation  result  only  in 
azimuth  errors  for  the  orientation  estimation  algorithms  associated  with  the  tested  sensor  modules.  Therefore,  in  the 
experiments  described  in  this  section,  only  magnetic  deviation  in  the  horizontal  plane  is  examined. 

To  measure  the  magnetic  deviation  in  the  horizontal  plane  caused  by  test  objects,  a  “track”  was  constructed  using 
non-ferrous  materials  and  set  so  that  the  orientation  of  an  inertial/magnetic  sensor  module  could  be  held  constant  as  the 
sensor  was  moved  through  successive  positions  approaching  each  object.  The  sensor  module  was  placed  at  18  locations 
with  each  successive  location  being  10  cm  closer  to  the  test  object.  In  the  final  position  the  sensor  module  was  within  one 
centimeter  of  the  test  object.  This  set-up  allowed  the  direction  of  the  magnetic  field  vector  to  be  measured  since  the  sensor 
module  orientation  was  kept  constant.  The  test  objects  included: 

•  Computer  monitor  (CRT  type),  powered  and  un-powered  states 

•  Simple  appliance  (small  space  heater  with  fan),  powered  and  un-powered  states 

•  Electrical  power  supply,  powered  and  un-powered  states 

•  Metal  filing  cabinet 

•  Mobile  robot,  un-powered,  powered,  and  motor  engaged. 

The  MicroStrain  3DM-G  sensor  module  is  factory  calibrated  and  allows  access  to  scaled  sensor  output  from  each  of 
the  nine  sensors  in  the  module.  The  magnetometer  triad  in  the  3DM-G  sensor  was  used  to  measure  the  magnetic  field 
direction  in  these  experiments. 

Prior  to  examination  of  the  deviations  caused  by  the  test  objects,  a  baseline  was  established  by  measuring  the  change 
in  magnetic  field  direction  with  no  object  present.  In  the  baseline  case,  the  direction  of  the  magnetic  field  in  the  horizontal 
plane  deviated  less  the  1.6°  as  the  sensor  module  was  moved  a  distance  of  180  cm  down  the  test  track.  This  deviation  is 
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attributed  to  noise  in  the  ambient  magnetic  field  of  the  laboratory.  Comparison  of  these  baseline  deviations  for  each 
sampling  position  to  the  deviations  that  occurred  when  each  of  the  test  objects  was  present  allows  a  more  thorough 
understanding  of  the  effects  each  object  has  on  the  magnetic  field.  The  baseline  was  sampled  before  and  after  the 
experiments  were  conducted.  This  action  helped  to  insure  that  no  significant  changes  had  occurred  in  the  ambient  magnetic 
field  of  the  laboratory  during  the  course  of  the  experiments. 

Figure  8  contains  two  sub-plots  of  data  from  experiments  in  which  a  CRT  computer  monitor  was  the  test  object.  In 
each  sub-plot,  baseline  average  deviations  are  displayed  along  with  the  average  deviations  that  occurred  when  the  monitor 
was  present.  The  top  sub-plot  displays  the  average  deviations  that  occurred  when  the  monitor  was  un-powered.  The  bottom 
sub-plot  displays  the  average  deviations  with  the  monitor  turned  on  and  connected  to  a  PC.  The  error  bars  represent  the 
standard  deviation  of  the  data  obtained  at  each  position.  The  magnetic  field  showed  approximately  the  same  amount  of 
deflection  whether  the  monitor  was  attached  to  a  PC  and  powered  up  or  turned  off.  The  standard  deviations  in  both 
experiments  are  small  and  can  be  attributed  to  measurement  noise,  indicating  the  deviation  was  a  DC  effect.  Some  impact 
from  this  appliance  can  be  observed  to  almost  40  cm  of  separation  distance.  In  both  cases,  the  computer  monitor  causes  a 
maximum  average  deflection  of  10.5°  in  the  magnetic  field  relative  to  the  horizontal  plane. 

Figure  9  shows  two  sub-plots  of  data  from  experiments  in  which  a  portable  heater  was  used  as  a  test  object.  In  the 
first  experiment  both  the  heater  fan  and  heating  elements  were  off.  In  the  second  experiment  both  the  fan  and  the  heating 
elements  were  on.  Examination  of  the  two  sub-plots  indicates  that  the  average  amount  of  magnetic  field  deviation  is 
significantly  greater  when  the  heater  is  turned  on  and  increases  dramatically  as  the  sensor  is  brought  in  close  proximity  to 
the  appliance.  The  standard  deviations  of  the  data  taken  at  each  position  also  increase  significantly  as  the  sensor  is  brought 
closer  to  the  running  heater.  This  fluctuation  is  most  likely  due  to  the  use  of  alternating  current  to  power  the  appliance. 
With  the  heater  in  a  powered  off  state,  the  largest  average  deviation  is  20.5°.  With  the  heater  turned  on,  the  largest  average 
deviation  is  nearly  90°.  In  both  cases  deviation  caused  by  the  heater  did  not  begin  to  occur  until  the  sensor  module  was 
within  30  cm  of  the  heater. 


Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 
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Figure  8:  Magnetic  field  vector  deviation  in  the  horizontal  plane  versus  distance  from  a  PC  monitor  in  both  un¬ 
powered  (top)  and  powered  (bottom)  states. 
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Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 


Figure  9:  Magnetic  field  vector  deviation  versus  distance  from  an  appliance  (space  heater)  in  both  un-powered  (top) 

and  powered  (bottom)  states. 

Figure  10  shows  two  sub-plots  of  data  from  experiments  in  which  an  electrical  power  supply  was  used  as  a  test 
object.  In  the  first  experiment  the  power  supply  is  off.  In  the  second  experiment  it  is  turned  on  and  supplying  power.  The 
two  sub-plots  are  very  similar.  The  standard  deviations  of  the  data  taken  at  each  position  are  relatively  small  again 
indicating  the  deviation  is  DC  in  nature.  In  both  sub-plots,  the  maximum  average  deviation  is  between  60°  and  70°.  The 
deviation  due  to  the  presence  of  the  power  supply  begins  to  occur  at  a  distance  of  nearly  one  meter. 

Figure  1 1  presents  the  deviation  in  the  sensed  magnetic  field  vector  as  the  magnetometer  triad  of  the  sensor  module 
approached  a  large  metal  filing  cabinet.  The  deviations  for  this  test  object  are  the  largest  of  any  observed  in  the 
experiments  described  in  this  paper.  Large  standard  deviations  for  the  data  samples  for  each  of  the  positions  are  not 
observed  indicating  that  the  magnetic  field  deviation  was  constant  in  nature.  The  maximum  deflection  caused  by  the  filing 
cabinet  is  99.5°  and  begins  at  a  distance  of  1.5  meters. 


Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 


Figure  10:  Magnetic  field  vector  deviation  versus  distance  from  an  electrical  power  supply  in  both  un-powered  (top) 

and  powered  (bottom)  states. 
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Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 


Figure  11:  Magnetic  field  vector  deviation  versus  distance  from  a  metal  filing  cabinet. 

The  final  test  object  presented  is  a  Nomad  Scout  mobile  robot.  Magnetic  deviation  due  to  the  presence  of  the  robot 
was  examined  with  the  robot  in  three  different  states.  These  three  states  correspond  to  the  three  sub-plots  in  Figure  12.  The 
bottom  sub-plot  displays  the  deviation  induced  by  the  robot  when  it  is  in  an  un-powered  state.  In  the  middle  sub-plot,  all 
electronic  systems  of  the  mobile  robot  were  energized  with  the  exception  of  the  motor  used  to  move  the  robot.  The  data 
displayed  in  the  top  sub-plot  were  collected  while  all  robot  systems  were  powered  and  the  motors  were  engaged.  The  robot 
was  placed  on  a  stand  so  that  its  wheels  could  rotate  freely.  The  maximum  amount  of  average  deviation  observed  for  the 
robot  is  about  9°.  This  includes  the  case  in  which  the  motors  were  engaged.  The  standard  deviation  of  the  data  for  each 
position  is  relatively  small.  No  deviation  is  observed  beyond  a  distance  of  40  cm. 


Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 


Distance  from  the  test  object  (centimeters) 


Figure  12:  Magnetic  field  vector  deviation  versus  distance  from  a  mobile  robot  in  a  power-off  (bottom),  systems-on 

(middle)  and  motor-engaged  (top)  states. 


Another  set  of  experiments  was  conducted  to  examine  the  amount  of  variation  that  can  be  expected  to  occur  in  a 
laboratory  environment  in  which  numerous  sources  of  magnetic  noise  are  present.  In  these  experiments,  the  azimuth 
direction  of  the  magnetic  field  was  measured  at  25  positions  at  10  cm  intervals  along  a  straight  line  with  the  sensor  module 
orientation  being  held  constant.  As  the  sensor  module  being  used  to  collect  measurement  data  was  moved  it  came  within 
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close  proximity  to  numerous  pieces  of  lab  equipment  simultaneously.  The  equipment  included  computer  monitors,  printers, 
mobile  robots,  servo  control  stations,  and  other  miscellaneous  lab  equipment.  Figure  13  contains  sub-plots  for  two  straight 
line  samples.  In  the  upper  sub-plot,  the  azimuth  direction  of  the  magnetic  field  varies  approximately  16°,  with  the 
maximum  change  between  two  adjacent  positions  being  13.1°.  In  the  lower  sub-plot,  the  azimuth  direction  of  the  magnetic 
field  varies  slightly  less  than  13°.  The  average  difference  from  position  to  position  is  less  than  three  degrees  for  both  trials. 
The  accruing  difference  in  the  magnetic  azimuth  direction  seen  in  the  upper  plot  indicates  the  presence  of  a  large  scale 
magnetic  disturbance  in  the  lab. 

Overall,  experiments  in  which  the  magnetic  field  variation  caused  by  individual  test  objects  was  examined  indicate 
that  when  inertial/magnetic  sensor  modules  are  separated  by  a  distance  of  one  meter  or  more  from  most  common 
appliances  and  ferrous  objects  the  amount  of  azimuth  error  induced  by  those  objects  will  be  negligible.  The  amount  of 
variation  caused  by  different  types  of  objects  can  vary  significantly.  The  experimental  results  demonstrate  that  while 
inclination  estimates  can  be  expected  to  remain  valid  in  close  proximity  to  objects  causing  distortions  in  the  local  magnetic 
field,  in  some  cases  the  azimuth  estimates  produced  by  the  implemented  algorithms  had  very  little  relation  to  the  true 
orientation  of  the  sensor  module  and  can  vary  by  as  much  as  100°.  In  other  tests,  azimuth  estimates  varied  less  that  10°. 
Experiments  in  which  sensor  modules  were  exposed  to  multiple  sources  of  distortion  simultaneously  in  a  crowded 
laboratory  environment,  show  that  azimuth  estimates  produced  using  a  sensor  module  with  a  constant  orientation  can  be 
significantly  different  for  closely  spaced  positions.  However,  on  average,  differences  in  estimated  azimuth  from  one 
position  to  another  nearby  position  are  much  smaller. 


Deviation  of  the  magnetic  field  vector  in  the  horizontal  plane 
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Figure  13:  Ambient  magnetic  field  azimuth  direction  sampled  at  10  cm  intervals  in  a  laboratory. 

3.3  Tracking  a  Robot  Arm 

The  final  set  of  experiments  described  in  this  paper  are  designed  to  determine  if  inertial/magnetic  sensor  modules  can  be 
used  to  accurately  track  the  orientation  of  the  links  of  a  robot  arm  made  of  ferrous  materials.  In  these  experiments  a 
SCORBOT-ER  III  robot  arm  and  three  MicroStrain  3DM-G  were  utilized.  The  experiments  described  in  section  3.1 
established  that  the  response  of  the  three  different  sensor  modules  to  magnetic  variations  is  essentially  the  same.  In  these 
tracking  experiments,  one  3DMG-GX1  was  securely  attached  to  each  link  of  the  arm.  While  robot  encoders  provide 
incremental  joint  angle  readings,  use  of  these  angles  to  obtain  orientation  estimates  relative  to  an  earth  fixed  reference 
frame  requires  forward  kinematics  and  calibration,  and  the  accuracy  of  the  orientation  estimates  cannot  be  ascertained  for 
this  robot  arm.  As  a  result  the  arm  was  also  tracked  using  a  Qualysis  optical  tracking  system  as  depicted  in  Figure  14.  The 
Qualysis  system  can  be  used  to  perform  passive  optical  three  degree-of-freedom  position  tracking  and  six  degree-of- 
freedom  tracking  of  designated  rigid  bodies  on  which  four  passive  markers  are  mounted.  Manufacturer’s  literature  states 
that  position  accuracy  is  0.1%  of  the  field  of  view.  The  robot  arm  was  contained  in  a  one  square  meter  tracking  volume. 
The  Qualysis  system  utilized  seven  proflex  cameras  positioned  around  the  tracking  volume.  In  the  experiments  discussed 
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here,  a  total  of  17  passive  markers  were  used  to  track  the  position  of  the  outboard  end  of  each  link  and  the  orientation  of 
the  inertial/magnetic  sensor  module  attached  to  each  link.  Each  link  was  defined  as  a  rigid  body  by  placing  four  markers  on 
the  surface  of  the  attached  inertial/magnetic  sensor.  The  geometric  center  of  these  four  markers  served  as  the  origin  of  the 
local  coordinate  system  of  each  segment.  Following  calibration,  maximum  residual  error  for  all  cameras  was  less  than 
1.127  mm.  These  calibration  results  indicate  that  the  system  was  tracking  to  1mm  accuracy  as  would  be  expected  given  the 
size  of  the  tracking  volume.  Update  rate  for  the  optical  tracking  system  was  60  Hz.  Given  this  accuracy,  data  produced  by 
this  system  was  treated  as  a  reference  in  these  experiments. 


Figure  14:  SCORBOT-ER  III  robot  arm  instrumented  for  tracking  with  both  inertial/magnetic  sensor  modules  and 

an  optical  tracking  system. 

Figure  15  shows  a  comparison  of  the  orientation  estimates  produced  using  an  inertial/magnetic  sensor  module  and  an 
optical  tracking  system  while  simultaneously  tracking  the  robot  arm.  During  the  experiment,  the  robot  arm  was 
programmed  to  repeatedly  trace  an  inclined  square  pattern  with  its  end  effecter.  Due  to  a  limited  number  of  degrees  of 
freedom  in  the  arm,  the  programmed  pattern  did  not  require  any  of  the  tracked  arm  segments  to  roll.  In  Figure  15,  yaw  and 
pitch  are  shown  for  the  most  outboard  end  inertial/magnetic  sensor.  Examination  of  the  Figure  15  shows  that  both  tracking 
technologies  produced  very  similar  motion  plots.  Maximum  steady  state  difference  between  the  orientation  estimates 
produced  using  inertial/magnetic  sensors  and  optical  tracking  is  less  than  2.5°  in  both  sub-plots.  This  accuracy  was 
achieved  by  the  inertial/magnetic  sensors  despite  the  largely  ferrous  nature  of  the  material  o  f  which  the  robot  arm  was 
constructed  and  the  presence  and  operation  of  several  servo  motors  used  to  position  and  move  the  arm. 


Yaw  Angle  Pitch  Angle 


Figure  15:  Comparison  of  yaw  and  pitch  orientation  estimates  for  a  robot  arm  segment  produced  by  an  optical 

tracking  system  and  an  inertial/magnetic  sensor  module. 
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4  Conclusions  and  Discussion 

The  direction  of  the  local  magnetic  field  vector  can  be  altered  by  the  presence  of  operating  electrical  appliances  or 
objects  made  of  ferrous  materials.  The  assumption  made  by  orientation  estimation  algorithms  that  the  direction  of  the  local 
magnetic  field  is  static  makes  the  algorithms  susceptible  to  errors  as  the  sensor  modules  are  moved  from  one  position  to 
another  within  a  tracking  volume.  In  the  algorithms  tested,  the  errors  appear  only  in  the  azimuth  portions  of  the  orientation 
estimates  produced.  These  errors  will  be  roughly  equal  in  size  to  the  amount  the  magnetic  field  deviates  in  the  horizontal 
plane  from  the  original  reference. 

The  amount  of  deviation  caused  by  appliances  and  ferrous  objects  can  range  from  very  small  to  very  large.  The 
horizontal  deviation  of  the  magnetic  field  was  measured  for  several  common  objects.  Maximum  deviation  ranged  from 
10.5  degrees  to  nearly  100  degrees.  Experimental  data  presented  here  indicates  that  such  deviations  can  be  largely  avoided 
by  maintaining  a  distance  of  approximately  one  meter  from  the  source  of  interference.  Only  one  of  the  objects  caused  a 
horizontal  plane  deviation  at  a  distance  of  more  than  one  meter.  For  this  object,  horizontal  plan  deviations  did  not  exceed  4 
degrees  when  at  a  distance  of  more  that  one  meter.  For  many  of  the  objects,  no  deviation  was  observed  beyond  a  distance 
of  a  half  meter.  However,  in  an  indoor  environment  containing  numerous  sources  of  interference,  it  can  be  difficult  to 
determine  which  objects  are  the  major  contributors  to  magnetic  field  deflections  and  the  magnetic  field  can  vary 
significantly  between  closely  spaced  positions. 

Despite  all  the  above,  the  tracking  experiments  indicate  that  inertial/magnetic  sensor  modules  can  be  used  to  track 
posture  with  an  accuracy  that  is  comparable  to  optical  tracking.  The  accuracy  of  the  orientation  estimates  while  tracking  a 
robot  arm  using  data  from  inertial/magnetic  sensor  modules  indicates  that  such  modules  can  be  used  to  accurately  track 
orientation  in  environments  and  applications  in  which  operating  motors  and  ferrous  objects  are  present.  However,  given  the 
current  state  of  the  art  of  orientation  estimation  algorithms  designed  to  process  inertial/magnetic  sensor  module  data,  they 
should  not  be  used  in  an  application  without  first  investigating  the  nature  of  the  magnetic  field  in  the  environment  in  which 
they  will  utilized.  While  Rotenberg  et  al.  have  begun  the  investigation  of  modified  algorithms  designed  to  alleviate  the 
effects  of  magnetic  variations  in  [23],  further  work  is  needed.  This  work  should  include  an  investigation  of  the  use  of 
arrays  of  sensor  modules  placed  at  slightly  different  positions  and  a  method  of  estimating  the  relative  amount  of 
interference  to  which  each  individual  module  is  exposed. 

The  findings  and  conclusions  in  this  report  are  those  of  the  authors  and  do  not  necessarily  represent  the  views  of  the 
National  Institute  for  Occupational  Safety  and  Health. 
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Abstract — Real-time  tracking  of  human  body  motion  is  an  im¬ 
portant  technology  in  synthetic  environments,  robotics,  and  other 
human-computer  interaction  applications.  This  paper  presents 
an  extended  Kalman  filter  designed  for  real-time  estimation  of 
the  orientation  of  human  limb  segments.  The  filter  processes  data 
from  small  inertial/magnetic  sensor  modules  containing  triaxial 
angular  rate  sensors,  accelerometers,  and  magnetometers.  The 
filter  represents  rotation  using  quaternions  rather  than  Euler 
angles  or  axis/angle  pairs.  Preprocessing  of  the  acceleration  and 
magnetometer  measurements  using  the  Quest  algorithm  produces 
a  computed  quaternion  input  for  the  filter.  This  preprocessing 
reduces  the  dimension  of  the  state  vector  and  makes  the  mea¬ 
surement  equations  linear.  Real-time  implementation  and  testing 
results  of  the  quaternion-based  Kalman  filter  are  presented. 
Experimental  results  validate  the  filter  design,  and  show  the 
feasibility  of  using  inertial/magnetic  sensor  modules  for  real-time 
human  body  motion  tracking. 

Index  Terms — Inertial  sensors,  Kalman  filtering,  magnetic  sen¬ 
sors,  motion  measurement,  orientation  tracking,  pose  estimation, 
quaternions,  virtual  reality. 


I.  Introduction 

MOTION  tracking  is  a  key  technology  in  synthetic  envi¬ 
ronments,  robotics,  and  other  applications  that  require 
real-time  information  about  the  motion  of  a  human.  A  number 
of  motion-tracking  technologies  have  been  developed  for  human 
motion  capture  in  virtual  reality  and  biomedical  applications, 
including  mechanical  trackers,  active  magnetic  trackers,  optical 
tracking  systems,  acoustic,  and  inertial/magnetic  tracking  sys¬ 
tems.  Most  are  dependent  on  an  artificially  generated  source  and 
are  thus  range-limited  and  susceptible  to  interference  and  noise. 

Mechanical  tracking  systems  can  be  placed  in  two  separate 
categories.  Body-based  systems  use  an  exoskeleton  that  is  at¬ 
tached  to  the  articulated  structure  to  be  tracked  [1] .  Goniometers 
within  the  skeletal  linkages  measure  joint  angles.  Ground-based 
systems  attach  one  end  of  a  boom  or  shaft  to  a  tracked  object  and 
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typically  have  six  degrees  of  freedom  (DOFs)  [2].  Ground-based 
systems  normally  only  track  a  single  rigid  body,  but  have  the  ad¬ 
vantage  of  being  able  to  provide  haptic  feedback. 

Practical  optical  tracking  systems  can  also  be  separated  into 
two  basic  categories.  Pattern  recognition  systems  sense  an  arti¬ 
ficial  pattern  of  lights  and  use  this  information  to  determine  po¬ 
sition  and/or  orientation  [3].  Such  systems  may  be  “outside-in” 
when  the  sensors  are  fixed  and  the  emitters  are  mobile,  or  “in¬ 
side-out”  when  sensors  are  mounted  on  mobile  objects  and  the 
emitters  are  fixed.  Image-based  systems  determine  position  by 
using  multiple  cameras  to  track  predesignated  points  on  moving 
objects  within  a  working  volume.  The  tracked  points  may  be 
marked  actively  or  passively  [4],  [5]. 

Active  magnetic  tracking  systems  determine  both  position 
and  orientation  by  using  sets  of  small  orthogonally  mounted 
coils  to  sense  a  set  of  sequentially  generated  magnetic  fields. 
The  sequentially  emitted  fields  induce  current  in  each  of  the 
sensor  coils,  allowing  measurement  of  orientation.  Changes  in 
total  strength  across  the  sensor  coils  are  proportional  to  the  dis¬ 
tance  from  the  field  transmitter  and  can  be  used  to  measure  po¬ 
sition  [6]. 

Ultrasonic  tracking  systems  can  determine  position  through 
either  time-of- flight  and  triangulation  or  phase-coherence. 
Phase-coherence  trackers  determine  distance  by  measuring  the 
difference  in  phase  of  a  reference  signal  and  an  emitted  signal 
detected  by  sensors. 

Body  tracking  using  inertial  and  magnetic  sensors  is  a  rela¬ 
tively  new  technology.  Inertial/magnetic  tracking  is  appealing 
due  to  a  lack  of  dependence  on  an  artificially  generated  source. 
It  thus  does  not  suffer  from  range  limitations  and  interference 
problems  of  sourced  technologies.  All  delay  or  latency  is  due  to 
data  processing  and  transmission.  The  availability  of  low-cost, 
small- size  micro-electro-mechanical  systems  (MEMS)  sensors 
has  made  it  possible  to  build  wrist- watch- sized,  self-contained 
inertial/magnetic  sensor  modules  [7],  [8].  These  modules  can 
be  used  to  accurately  track  orientation  in  real  time.  Attachment 
of  such  sensor  modules  to  each  of  the  major  limb  segments  of 
a  human  makes  it  possible  to  independently  determine  the  ori¬ 
entation  of  each  segment  relative  to  an  Earth-fixed  reference 
frame.  The  human  model  is  constructed  from  multiple  indepen¬ 
dently  oriented  limb  segments  that  are  constrained  by  their  at¬ 
tachment  to  each  other.  Relative  orientation  between  limb  seg¬ 
ments  is  not  determined  or  needed. 

A  naive  approach  to  inertial  orientation  tracking  might  in¬ 
volve  integration  of  angular  rate  data  to  determine  orientation. 
However,  this  solution  would  be  prone  to  drift  over  time  due  to 
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the  buildup  of  bias  and  drift  errors.  In  order  to  avoid  drift,  in¬ 
ertial  tracking  systems  make  use  of  additional  complementary 
sensors.  Commonly,  these  sensors  include  triads  of  accelerome¬ 
ters  and  magnetometers  for  respectively  referencing  the  gravity 
and  magnetic  field  vectors.  Measuring  the  gravity  vector  in  the 
sensor  coordinate  frame  using  accelerometers  allows  estimation 
of  orientation  relative  to  the  horizontal  plane.  However,  in  the 
event  that  the  sensor  module  is  rotated  about  the  vertical  axis,  the 
projection  of  the  gravity  vector  on  each  of  the  principal  axes  of 
the  accelerometer  triad  will  not  change.  Since  the  accelerometer 
triad  can  not  be  used  to  sense  a  rotation  about  the  vertical  axis, 
magnetometers  are  used  to  measure  the  local  magnetic  field 
vector  in  sensor  coordinates  and  allows  determination  of  ori¬ 
entation  relative  to  the  vertical.  The  data  from  the  incorporated 
sensors  is  normally  fused  using  a  Kalman  or  complementary 
filtering  algorithm.  It  should  be  noted  that  data  from  low-cost 
MEMS  accelerometers  cannot  be  double-integrated  for  an  ex¬ 
tended  period  of  time  to  determine  position,  due  to  a  quadratic 
growth  of  errors. 

This  paper  describes  the  design,  implementation,  and  experi¬ 
mental  testing  of  an  extended  Kalman  filter  (EKF)  for  real-time 
tracking  of  human  body  motion.  In  order  to  produce  3-D  orien¬ 
tation  estimates  relative  to  an  Earth-fixed  reference  frame,  the 
filter  uses  input  data  from  a  sensor  module  containing  a  triad  of 
orthogonally  mounted  linear  accelerometers,  a  triad  of  orthogo¬ 
nally  mounted  angular  rate  sensors,  and  a  triad  of  orthogonally 
mounted  magnetometers.  Quaternions  are  used  to  represent  ori¬ 
entation  to  improve  computational  efficiency  and  avoid  singu¬ 
larities.  In  addition,  the  use  of  quaternions  eliminates  the  need 
for  computing  trigonometric  functions.  The  filter  continuously 
corrects  for  drift  based  on  the  assumption  that  human  limb  ac¬ 
celeration  is  bounded,  and  averages  to  zero  over  any  extended 
period  of  time.  A  first-order  linear  system  is  used  to  model 
human  body  limb  segment  motion.  The  QUEST  algorithm  is 
used  to  preprocess  accelerometer  and  magnetometer  measure¬ 
ments,  resulting  in  a  significant  simplification  of  the  Kalman 
filter  design.  The  filter  is  experimentally  validated  using  actual 
sensor  measurements. 

The  primary  contributions  of  this  paper  are: 

•  analysis  that  determines  that  a  simple  motion  model  based 
on  a  first-order  linear  system  is  sufficient  for  tracking 
human  limb  segment  orientation; 

•  an  EKF  designed  for  tracking  human  limb  segment  orien¬ 
tation  that  fuses  a  precalculated  quaternion  input  with  an¬ 
gular  rate  data; 

•  experimental  results  validating  that  filter  performance  is 
adequate  for  human  posture  tracking  applications. 

The  paper  is  organized  as  follows.  Section  II  provides  an 
overview  of  related  work,  and  contrasts  that  with  the  approach 
described  in  this  paper.  Section  III  gives  a  brief  description  of 
the  MARG  sensors  used  to  obtain  experimental  data.  Section  IV 
presents  the  process  model  of  the  Kalman  filter  for  human  body 
motion  tracking.  Section  V  describes  two  approaches  to  Kalman 
filter  design.  Section  VI  describes  implementation  issues  of  the 
Kalman  filter  with  a  focus  on  how  the  nonlinear  process  model 
was  first  linearized  and  then  discretized.  Experimental  modeling 
of  the  process  noise  covariance  matrix  and  the  measurement 
noise  covariance  matrix  is  also  detailed.  Section  VII  reports  the 


MATLAB  simulation  and  offline  testing  results  of  the  Kalman 
filter.  Section  VIII  describes  the  real-time  implementation  of  the 
algorithm  and  testing  results.  The  final  section  provides  a  sum¬ 
mary  and  conclusions. 

II.  Related  Work 

Many  studies  of  human  motion  tracking  using  inertial  sensors 
have  been  performed.  Depending  on  the  type,  number,  and  con¬ 
figuration  of  sensors  used,  some  studies  are  limited  to  tracking 
two  degrees  of  orientation  in  a  plane,  while  others  track  3-D 
orientation.  Algorithms  have  also  been  designed  to  track  limb 
segment  orientations  relative  to  each  other  or  calculate  joint  an¬ 
gles,  as  opposed  to  estimating  the  orientation  of  a  limb  segment 
relative  to  an  Earth-fixed  reference  frame. 

A  study  of  human  motion  tracking  using  accelerometers 
alone  was  reported  in  [9].  During  motions  involving  small 
linear  accelerations,  a  set  of  triaxial  accelerometers  was  used 
to  determine  joint  angles.  During  motions  accompanied  by 
higher  accelerations,  a  technique  is  described  that  involves  the 
use  of  two  sets  of  triaxial  accelerometers  on  a  single  rigid  body 
to  differentiate  gravitational  acceleration  from  motion-related 
linear  acceleration.  Though  the  effects  of  these  geometric 
sensor  fusion  techniques  are  depicted,  there  is  no  comparison 
with  truth  data.  The  use  of  magnetometers  is  mentioned,  but 
not  discussed.  Rehbinder  and  Hu  [10]  describe  an  attitude 
estimation  algorithm  based  on  the  use  of  angular  rate  sensors 
and  accelerometers.  In  this  paper,  drift  in  heading  estimation 
was  unavoidable  due  to  a  lack  of  additional  complementary 
sensors,  such  as  magnetometers.  Thus,  only  two  DOFs  of 
orientation  are  tracked.  Sabatini  et  al.  [11]  used  a  single  sensor 
module  containing  a  biaxial  accelerometer  and  one  gyroscope 
to  perform  gait  analysis  and  measurement.  To  measure  incline, 
distance,  and  speed,  the  method  exploits  the  cyclical  features 
of  human  gait.  Transition  from  one  gait  phase  to  the  next  is 
determined  using  gyroscope  data.  Acceleration  data  is  double 
integrated  during  the  swing  phase  to  determine  position  and 
used  to  determine  the  vertical  when  the  foot  is  flat  on  the 
ground.  Since  the  accelerometers  are  unable  to  detect  rotations 
about  the  vertical  plane,  all  motion  is  assumed  to  take  place 
in  a  nonrotating  sagittal  plane.  Sabatini  [12]  took  this  research 
further  by  creating  a  quaternion-based  filtering  algorithm. 
A  quaternion  interpolation  technique  is  used  to  improve  the 
accuracy  of  orientation  and  position  estimates  by  reducing 
the  effects  of  sensor  bias  and  scale  factor  drift  in  both  the 
accelerometers  and  gyroscope.  Unlike  the  work  described  in 
this  paper,  this  gait  analysis  work  does  not  attempt  to  measure 
posture.  In  similar  gait  measurement  work,  Veltink  et  al  [13] 
use  a  sensor  module  containing  a  three-axis  accelerometer  and 
a  three-axis  angular  rate  sensor  to  measure  gait  characteristics 
in  order  to  tune  an  implantable  drop-foot  simulator. 

In  a  study  of  dynamic  registration  in  augmented  reality  ap¬ 
plications  that  require  more  precise  orientation  tracking  as  well 
as  position  tracking,  Azuma  and  Bishop  [14]  use  inertial  data 
from  linear  accelerometers  and  angular  rate  sensors  to  reduce 
apparent  lag  in  the  position  and  orientation  estimates  produced 
by  an  optoelectronic  tracking  system.  The  use  of  an  EKF  pre¬ 
dictor  resulted  in  errors  5-10  times  lower  than  without  predic- 
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tion.  In  contrast,  the  work  described  in  this  paper  produces  only 
estimates  of  orientation  using  inertial  and  magnetic  data. 

Full  3 -DOF  orientation  tracking  is  most  commonly  per¬ 
formed  using  nine-axis  sensor  modules  containing  three 
orthogonally  mounted  triads  of  angular  rate  sensors,  ac¬ 
celerometers,  and  magnetometers.  Foxlin  et  al.  [15],  [16] 
describes  two  commercial  nine-axis  sensing  systems  designed 
for  head  tracking  applications.  Sensor  fusion  is  performed  using 
a  complementary  separate-bias  Kalman  filter.  Drift  correction 
is  described  as  only  being  performed  during  stationary  periods 
when  it  is  assumed  accelerometers  are  sensing  only  gravita¬ 
tional  acceleration.  Thus,  the  described  algorithm  requires  that 
all  motion  stop  in  order  to  correct  inertial  drift  errors. 

Bachmann  et  al.  [7],  [17]  proposed  a  nonoptimal  quaternion- 
based  complementary  filter  for  human  body  tracking.  The  filter 
is  able  to  track  through  all  orientations  without  singularities,  and 
continuously  correct  for  drift  without  a  need  for  stationary  pe¬ 
riods  using  nine-axis  inertial/magnetic  sensor  module  data.  Ex¬ 
tensions  to  this  work  and  the  development  of  an  optimal  filter  de¬ 
signed  for  human  posture  tracking  applications  are  described  in 
[  1 8]-[20] .  Use  of  a  first-order  linear  system  for  modeling  human 
body  limb  motions  was  first  proposed  in  [18].  A  Gauss-Newton 
iteration  method  is  used  to  preprocess  accelerometer  and  mag¬ 
netometer  data  to  produce  quaternion  input  to  the  EKF.  Formu¬ 
lation  and  simulation  testing  of  a  reduced-order  implementation 
of  the  Gauss-Newton  iteration  method  for  this  Kalman  filter  is 
documented  in  [19].  Preliminary  experimental  testing  results  are 
presented  in  [20] . 

In  [21],  Gallagher  et  al.  present  a  nonoptimal  complemen¬ 
tary  filter  algorithm  that  has  a  lower  computational  complexity 
and  similar  accuracy  to  the  work  described  by  Bachmann  et 
al.  in  [7]  and  [17].  Luinge  describes  a  Kalman  filter  designed 
for  human  body  tracking  application  in  [22]-[24].  In  the  pro¬ 
posed  method,  inclination  is  determined  without  low-pass  fil¬ 
tering  accelerometer  data.  The  design  is  based  on  assumptions 
concerning  the  frequency  content  of  the  acceleration  and  the 
magnitude  of  gravity.  Reduction  of  drift  about  the  vertical  axis 
is  dependent  on  the  use  of  a  kinematic  human  body  model.  Mag¬ 
netometers  are  not  used.  More  recently,  Roetenberg  et  al.  [25] 
extended  the  Kalman  filter  described  in  [23]  to  include  a  mag¬ 
netometer  model  designed  to  prevent  heading  drift  and  com¬ 
pensate  for  magnetic  disturbances.  This  compensation  allowed 
a  significant  estimation  accuracy  improvement  in  comparison 
with  no  compensation  or  using  angular  rate  sensors  only.  In 
[26],  Zhu  and  Zhou  describe  a  linear  Kalman  filter  algorithm 
designed  to  smooth  accelerometer  and  magnetometer  readings 
from  a  nine-axis  sensor  module.  Rather  than  estimating  indi¬ 
vidual  limb  segment  orientations  relative  to  a  fixed  reference 
frame,  as  is  done  in  this  paper,  their  system  determines  joint 
angles  in  axis/angle  form  using  the  data  from  the  two  sensors 
mounted  on  the  inboard  and  outboard  sides  of  the  joint.  The  axis / 
angle  pairs  are  determined  analytically  using  processed  mea¬ 
surement  data. 

Kraft  [27]  describes  an  “unscented,”  quaternion-based 
Kalman  filter  for  real-time  estimation  of  rigid-body  orientation 
using  nine-axis  sensor  modules.  The  described  filter  approx¬ 
imates  the  Gaussian  probability  distribution  using  a  set  of 
sample  points  instead  of  linearizing  nonlinear  process  model 


IEEE  TRANSACTIONS  ON  ROBOTICS,  VOL.  22,  NO.  6,  DECEMBER  2006 

equations.  Simulation  results  demonstrate  the  general  validity 
of  the  described  filter.  Tests  of  the  filter  with  real  measure¬ 
ments  are  mentioned,  but  not  shown  or  quantified.  Haid  and 
Breitenbach  [28]  also  describe  a  Kalman  filter  algorithm  for 
use  with  inertial  and  magnetic  sensors.  The  primary  aim  of 
the  filter  is  the  elimination  of  drift  and  bias  effects  observed 
in  low-cost  angular  rate  sensors.  The  filter  works  only  in  the 
single  dimension  of  the  targeted  angular-rate  sensor.  It  does  not 
estimate  limb  segment  orientation  or  joint  angles. 

Some  work  has  attempted  to  eliminate  the  need  to  include  an¬ 
gular  rate  sensors  in  inertial/magnetic  sensor  modules.  In  [29], 
Gebre-Egziabher  et  al.  describe  an  attitude  determination  al¬ 
gorithm  for  aircraft  applications.  The  algorithm  is  based  on  a 
quaternion  formulation  of  Wahba’ s  problem  [30],  where  magne¬ 
tometer  and  accelerometer  measurements  are  used  to  determine 
attitude  without  the  use  of  angular  rate  sensors.  A  Kalman  filter 
implementation  of  the  algorithm  is  also  presented.  The  algo¬ 
rithm  is  based  on  the  assumption  that  the  rigid  body  to  which  the 
sensor  is  attached  is  stationary  or  is  slow  moving,  and  is  thus  not 
applicable  to  highly  dynamic  tracking  applications.  Chin- Woo 
et  al.  [31]  propose  a  gyroscope  free  inertial  navigation  system 
that  uses  accelerometers  to  determine  both  linear  and  angular 
motions  of  a  rigid  body.  The  approach  requires  a  minimum  of 
six  accelerometers.  Acceptable  configurations  and  basic  algo¬ 
rithms  are  examined  through  simulation.  Use  of  accelerome¬ 
ters  to  calculate  angular  rate  results  in  a  faster  orientation  error 
growth  rate  than  that  associated  with  conventional  angular  rate 
sensors.  This  result  is  due  to  inclusion  of  the  angular  accelera¬ 
tion  terms  which  introduce  integrated  noise  and  drift.  The  idea 
of  using  accelerometers  to  measure  angular  rate  is  carried  fur¬ 
ther  by  Ang  et  al.  in  [32]  and  [33]. 

In  contrast  with  the  work  described  above,  this  paper  presents 
a  filter  algorithm  that  is  specifically  designed  for  tracking 
human-limb  segment  orientation  relative  to  an  Earth-fixed 
frame.  The  algorithm  incorporates  a  human  body  motion 
model.  It  adopts  a  two-layer  filter  architecture,  in  which  the 
QUEST  algorithm  preprocesses  accelerometer  and  magne¬ 
tometer  data  and  an  EKF  fuses  the  QUEST  output  with  angular 
rate  data. 

III.  MARG  Sensors 

Experimental  data  were  collected  using  MARG  III  iner¬ 
tial/magnetic  sensor  modules  designed  by  the  authors  and 
fabricated  by  McKinney  Technology  [8].  The  MARG  sensor 
design  is  based  on  its  primary  application,  that  is,  human  body 
motion  tracking.  Primary  sensing  components  for  this  unit 
include  Tokin  CG-L43  ceramic  rate  gyros,  Analog  Devices 
ADXL202E  micromachined  accelerometers,  and  Honeywell 
HMC1051Z  and  HMC1052  one-  and  two-axis  magnetometers. 
The  sensor  module  also  incorporates  a  Texas  Instruments 
MSP430F149  ultra-low-power,  16-bit  RISC  architecture  mi¬ 
crocontroller.  Overall,  dimensions  of  the  MARG  III  unit  are 
approximately  1.8  cm x 3.0  cm x 2. 5  cm. 

The  manufacturer  specified  maximum  allowable  angular  rate 
of  the  CG-L43  ceramic  gyro  is  ±  90°/s.  This  is  deemed  suffi¬ 
cient  to  quicken  response  in  human  body  motion  tracking  appli¬ 
cations,  but  not  accurately  measure  rates  associated  with  highly 
dynamic  motion.  Three  of  these  gyros  are  orthogonally  mounted 
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Fig.  1.  Kalman  filter  process  model,  q  is  the  orientation  quaternion,  to  is  the  angular  velocity,  w  is  a  white  noise,  and  r  is  the  time  constant. 


within  the  MARG  unit  to  form  a  triad  capable  of  measuring 
3 -DOF  angular  rate.  In  the  results  presented  here,  the  limita¬ 
tions  of  the  angular  rate  sensors  did  not  affect  the  performance 
or  demonstration  of  the  algorithms  during  typical  human  mo¬ 
tion. 

The  maximum  measurement  range  of  Analog  Devices 
ADXL202E  is  =b  2  g,  which  is  acceptable  for  sensing  gravita¬ 
tional  acceleration.  The  ADXL202E  is  a  two-axis  acceleration 
sensor  on  a  single  chip.  As  a  result,  only  two  of  them  are  re¬ 
quired  to  form  a  triad  for  measuring  3 -DOF  acceleration.  The 
ADXL202E  also  offers  a  duty  cycle  output,  which  can  be  directly 
interfaced  to  a  low-cost  microcontroller  without  analog/digital 
(A/D)  converters.  The  accelerometers  are  not  used  to  measure 
linear  accelerations  associated  with  human  motion. 

In  the  MARG  design,  a  one-axis  HMC1051Z  for  the  z-axis 
and  a  two-axis  HMC1052  for  x-y  axes  are  mounted  on  the  same 
PCB  to  form  a  three-axis  magnetometer.  The  HMC1051Z  and 
HMC1052  are  specially  designed  to  be  mounted  on  the  same 
PCB  to  form  an  orthogonal  triad. 

The  purpose  of  the  microcontroller  is  to  convert  analog  sensor 
outputs  to  digital  data,  digitally  filter  the  angular  rate  sensor 
data,  and  perform  automatic  set/reset  of  magnetometers  to  avoid 
magnetic  saturation  problems.  To  prepare  angular  rate  data  for 
processing  by  the  Kalman  filter,  the  data  is  averaged  on  start 
up  to  establish  initial  bias  values.  During  run-time,  angular  rate 
data  is  preprocessed  using  a  simple  first-order  high-pass  filter 
to  eliminate  drift  over  time.  Static  bench  tests  have  established 
that  accelerometer  and  magnetometer  data  are  relatively  stable 
over  time,  and  they  are  thus  not  bias -corrected  at  run-time.  Mag¬ 
netic  interference  is  a  major  concern  when  using  magnetome¬ 
ters  in  environments  containing  changing  or  distorted  magnetic 
fields  [34] .  This  is  an  active  area  of  research  by  the  authors  and 
others  [25].  No  compensation  for  external  magnetic  effects  is 
performed  in  the  work  described  in  this  paper.  It  is  noted  that  the 
MARG  sensor  is  a  prototype  constructed  using  2  g  accelerom¬ 
eters  and  90°/s  angular  rate  sensors.  These  components  were 
chosen  for  typical  human  motion,  and  may  not  be  sufficient  for 
extreme  human  motion.  The  algorithm  presented  in  the  paper  is 
not  limited  to  these  particular  sensor  parameters. 

IV.  Kalman  Filter  Process  Model 

As  stated  above,  the  objective  of  this  paper  is  to  design  a 
Kalman  filter  for  real-time  tracking  of  human  body  motion.  To 
do  so,  it  is  necessary  to  establish  a  process  model  representing 
motion  dynamics  of  the  human  musculoskeletal  system.  Dy¬ 
namic  models  of  the  human  musculoskeletal  systems  are  com¬ 
plex,  and  have  been  studied  for  many  years.  Such  models  are 
ideal  for  computer  simulations  of  articulated  body  motions,  but 
remain  too  computationally  demanding  for  real-time  applica¬ 
tions  such  as  real-time  human  motion  tracking.  Thus,  the  chal¬ 
lenge  is  to  develop  a  model  that  is  simple  yet  adequate  for  mo¬ 
tion  tracking  applications.  Based  on  extensive  trial  and  error 


study,  a  first-order  linear  system  model  is  adopted  to  represent 
the  motion  of  each  human  body  limb  segment.  Such  a  model  is 
depicted  in  the  left  half  of  Fig.  1.  It  is  assumed  that  each  limb 
segment  is  independent  of  the  others.  The  input  to  the  linear 
system  is  a  white  noise  w,  and  the  output  is  the  angular  velocity 
uj  of  the  limb  segment.  The  most  important  parameter  in  this 
model  is  the  time  constant  r,  which  determines  how  fast  a  limb 
segment  (e.g.,  upper  arm)  can  move  in  typical  human  motion 
conditions.  The  angular  velocity  is  thus  modeled  as  a  colored 
noise  generated  by  a  linear  system  with  a  white  noise  input. 

In  the  filter,  quaternions  are  used  to  represent  the  orientation 
of  each  body  limb  segment  for  two  reasons.  First,  the  quaternion 
representation  does  not  suffer  from  the  singularity  problem  as¬ 
sociated  with  the  Euler  angle  representation.  Second,  it  avoids 
trigonometric  functions  in  the  filter  algorithm,  making  it  more 
efficient  and  easier  to  implement  in  real  time  on  microcon¬ 
trollers.  In  what  follows,  q  will  be  used  to  denote  the  orientation 
quaternion  in  Earth  coordinates.  The  angular  velocity  uj  and  the 
quaternion  derivative  q  are  related  by  the  following  well-known 
identity  [35]: 


where  0  represents  quaternion  multiplication.  Equation  (1)  is 
represented  by  the  center  block  in  Fig.  1 .  The  quaternion  deriva¬ 
tive  q  is  integrated  to  produce  the  quaternion  q.  In  order  to  take 
advantage  of  computational  simplifications  and  efficiencies  as¬ 
sociated  with  unit  quaternions,  the  resultant  quaternion  is  nor¬ 
malized  to  unit  length  in  the  last  step  of  the  process  model,  as 
shown  in  Fig.  1.  The  quaternion  q  produced  by  the  integrator 
may  not  be  exactly  unit  length,  but  it  is  normally  very  close  to  a 
unit  quaternion.  To  avoid  the  complexity  that  the  normalization 
introduces  into  the  Kalman  filter  derivation,  it  is  not  included  in 
the  process  model  equations  presented  in  the  next  section.  As  a 
result,  although  the  Kalman  filter  is  an  optimal  algorithm,  this 
normalization  procedure  leads  to  a  suboptimal  algorithm.  In  the 
next  section,  two  Kalman  filter  designs  based  on  this  process 
model  will  be  presented. 

V.  Kalman  Filter  Design 

Two  alternative  approaches  to  the  Kalman  filter  design  based 
on  the  process  model  presented  in  Section  IV  will  be  described 
in  this  section.  The  state  vector  for  both  approaches  is  the  same. 
It  is  a  7-D  vector  consisting  of  the  three  components  of  angular 
rate  and  the  four  elements  of  the  orientation  quaternion.  The 
difference  between  the  two  approaches  is  in  the  measurement 
or  output  equation  for  the  Kalman  filter.  The  first  approach  uses 
a  standard  Kalman  filter  design,  which  has  a  9-D  measurement 
vector,  consisting  of  3-D  angular  rate,  3-D  acceleration,  and  3-D 
local  magnetic  field.  This  9-D  vector  directly  corresponds  to 
the  measurements  provided  by  inertial/magnetic  sensors  mod¬ 
ules.  The  first  three  components  of  the  output  equation  (angular 
rate  portion)  are  linearly  related  to  the  state  vector.  However,  the 
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other  six  components  of  the  output  equation  are  nonlinearly  re¬ 
lated  to  the  state  vector.  The  nonlinear  relationship  is  quite  com¬ 
plicated.  As  a  result,  the  EKF  designed  with  this  output  equation 
is  computationally  inefficient. 

The  second  approach  uses  a  separate  algorithm  to  find  a 
corresponding  quaternion  for  each  set  of  accelerometer  and 
magnetometer  measurements.  The  computed  quaternion  is  then 
combined  with  the  angular  rate  measurements,  and  presented  to 
the  Kalman  filter  as  its  measurements.  By  doing  so,  the  output 
equations  for  the  Kalman  filter  become  linear,  and  the  overall 
Kalman  filter  design  is  greatly  simplified. 

A.  The  First  Approach 

The  first  approach  is  a  standard  Kalman  filter  design  based  on 
the  process  model  depicted  in  Fig.  1.  The  state  vector  x  is  7-D, 
with  the  first  three  components  being  the  angular  rate  u ;,  and  the 
last  four  components  being  the  quaternion  q.  That  is 
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Fig.  2.  Block  diagram  of  the  first  approach  to  Kalman  filter  design. 
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Fig.  3.  Block  diagram  of  the  second  approach  to  Kalman  filter  design. 
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It  is  noted  that  quaternion  normalization  is  not  modeled  in  these 
state  equations,  but  is  carried  out  in  the  real-time  implementa¬ 
tion. 

Since  measurement  data  to  the  filter  are  provided  by  MARG 
sensors,  it  is  natural  to  choose  the  following  as  the  measure¬ 
ments  of  the  Kalman  filter: 


Zl 

x  component  of  angular  rate 

Z2 

= 

y  component  of  angular  rate 

_Z?>_ 

%  component  of  angular  rate 

^4 

x  component  of  acceleration 

^5 

= 

y  component  of  acceleration 

z  component  of  acceleration 

z7 

x  component  of  local  magnetic  field 

^8 

= 

y  component  of  local  magnetic  field 

%  component  of  local  magnetic  field 

Since  angular  rates  are  part  of  the  state,  the  first  three  measure¬ 
ment  equations  are  simply  given  by  the  following: 


Ml  =  +  V!  (4) 

z2=  x2  +  v2  (5) 

Z3  =  %3  +  V3  (6) 

where  Vi  is  the  measurement  noise  that  is  assumed  to  be  white. 
As  for  the  remaining  six  measurement  equations,  they  turn  out  to 


be  quite  complicated.  As  an  example,  the  seventh  measurement 
equation  is  given  by 

z7  =  ((xl  +  x7  -  x%  -  x%)  /hi  +  2(x4x5  -  x6x7)/h2 

+2(x4x6  +  x5x7)/h3)  /  (x4  +  x\  +  XqX?)  +  v4  (7) 

where  h\,h2,  and  hs  are  values  of  the  Earth  magnetic  field 
measured  in  the  Earth  coordinates,  which  are  constant  for  a 
given  location.  It  is  not  difficult  to  design  an  EKF,  as  shown 
in  Fig.  2,  based  on  state  (2)  and  (3),  and  the  nine  measurement 
equations,  which  was  indeed  carried  out  in  [36].  The  problem  is 
that  computational  requirements  for  implementing  such  a  filter 
are  extremely  high,  making  it  unfeasible  for  real-time  motion 
tracking.  An  alternative  approach  to  the  Kalman  filter  design  is 
thus  presented  in  the  next  subsection. 

B.  The  Second  Approach 

Fig.  3  shows  a  block  diagram  of  an  alternative  approach  to 
filter  design.  Acceleration  and  local  magnetic  field  measure¬ 
ments  are  used  as  input  to  the  QUEST  algorithm  [37]  to  produce 
what  will  be  called  the  computed  quaternion.  The  computed 
quaternion  together  with  angular  rate  measurements  is  then  pre¬ 
sented  to  a  Kalman  filter  as  measurements.  It  will  be  seen  below 
that  the  Kalman  filter  in  this  case  is  significantly  simpler,  owing 
to  the  fact  that  the  measurement  equations  are  linear.  It  is  true 
that  there  is  an  additional  computational  cost  to  implement  the 
QUEST  algorithm  in  this  approach.  Still,  the  overall  computa¬ 
tional  requirements  for  this  approach  are  much  less  than  what  is 
needed  for  the  first  approach. 

The  QUEST  (quaternion  estimator)  algorithm  is  a  popular  al¬ 
gorithm  for  a  single-frame  estimation  of  an  attitude  quaternion 
[37].  The  algorithm  was  created  to  solve  Wahba’s  problem  [30] 
that  involved  determination  of  the  attitude  of  a  rigid  body  in  ref¬ 
erence  to  a  fixed  coordinate  system  based  on  a  set  of  measure¬ 
ment  or  observation  vectors  using  a  closed  form  solution.  The 
minimum  number  of  measurement  vectors  required  to  compute 
orientation  is  two.  Early  solutions  to  Wahba’s  problem  directly 
compute  a  rotation  matrix  capable  of  rotating  the  measurement 
(assuming  no  errors)  vectors  to  match  the  reference  vectors.  The 
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QUEST  algorithm  solves  Wahba’s  problem  by  calculating  the 
four  elements  of  the  corresponding  optimal  quaternion  [37]. 

This  alternative  approach  to  filter  design  as  shown  in  Fig.  3 
is  not  without  reasons.  If  the  limb  segment  to  which  an  iner¬ 
tial/magnetic  sensor  module  is  attached  is  stationary,  accelera¬ 
tion  (gravity)  and  local  magnetic  field  measurements  are  suffi¬ 
cient  to  determine  the  orientation  of  the  body.  While  stationary, 
accelerometers  measure  the  local  gravity  vector  in  the  body 
frame.  The  3-D  gravity  measurements  can  be  used  to  determine 
roll  and  pitch  angles  of  the  body  relative  to  the  fixed  Earth  frame. 
The  yaw  angle  of  the  body  is  determined  from  the  local  mag¬ 
netic  field  measurements.  In  this  application,  the  QUEST  al¬ 
gorithm  takes  gravity  and  magnetic  field  measurement  vectors 
with  equal  weight  and  computes  the  optimal  quaternion  that  will 
rotate  these  vectors  to  match  their  corresponding  reference  vec¬ 
tors. 

While  the  rigid  body  is  in  motion,  the  computed  quaternions 
from  this  algorithm  do  not  represent  the  actual  real-time  ori¬ 
entation  of  the  body,  because  accelerometers  measure  the  sum 
of  gravity  and  motion  induced  acceleration.  This  is  where  an¬ 
gular  rate  measurements  come  to  help  estimate  the  orientation 
of  the  rigid  body.  While  angular  rate  measurements  can  be  in¬ 
tegrated  to  yield  an  orientation  estimate,  they  are  prone  to  drift 
over  an  extended  period  of  time.  Acceleration  and  magnetic  field 
measurements  do  not  drift  over  time.  The  Kalman  filter  in  this 
approach  is  designed  to  optimally  fuse  the  complementary  in¬ 
formation  provided  by  the  angular  rate  measurements  and  the 
computed  quaternion. 

It  should  be  pointed  out  that  this  filtering  architecture  has 
been  previously  proposed  and  successfully  applied  in  other 
areas  such  as  attitude  heading  and  reference  systems  (AHRS) 
[38].  In  [38],  an  inertial  navigation  system  for  autonomous 
underwater  vehicles  was  developed,  in  which  a  complementary 
filter  first  combines  measurement  data  from  accelerometers, 
angular  rate  sensors,  and  magnetic  sensors.  An  EKF  then  fuses 
the  output  of  the  complementary  filter  with  the  GPS/DGPS 
measurements. 

The  state  equations  in  the  second  approach  are  the  same  as 
those  in  the  first  approach,  that  is,  equations  (2)  and  (3).  The 
measurement  equations  in  this  case  are  much  simpler,  and  they 
are 


Zi  =  Xi  +  Vi,  i  =  1, . . . ,  7  (8) 

where  V{  is  the  white  noise  measurement.  Although  the  mea¬ 
surement  equations  are  linear,  an  EKF  is  still  required  since  the 
second  part  of  the  state  (3)  is  nonlinear.  Nevertheless,  linearity 
in  the  measurement  equations  significantly  simplifies  the  filter 
design  and  reduces  computational  requirements  for  real-time 
implementation. 

C.  Discussion 

The  first-order  process  model  and  an  early  version  of  the 
second  approach  to  the  Kalman  filter  design  was  first  reported  in 
[18].  Rather  than  using  the  QUEST  algorithm,  a  Gauss-Newton 
iteration  method  was  used  to  preprocess  accelerometer  and 
magnetometer  data  to  produce  quaternion  input  to  the  EKF.  A 


reduced-order  implementation  of  the  Gauss-Newton  iteration 
method  was  described  in  [19].  This  reduced  order  implemen¬ 
tation  requires  computing  the  inverse  of  a  3x3  matrix  rather 
than  that  of  a  4x4  matrix.  The  Gauss-Newton  iteration  method 
was  replaced  by  the  factored  quaternion  algorithm  in  [20]. 
While  more  efficient,  the  factored  quaternion  algorithm  pro¬ 
vides  a  suboptimal  solution.  The  QUEST  algorithm  provides 
an  optimal  solution  for  noisy  measurement  data.  Preliminary 
experimental  results  were  also  reported  in  [20].  In  this  paper, 
the  QUEST  algorithm  is  adopted  to  preprocess  the  acceleration 
and  magnetic  field  measurement  data.  The  QUEST  algorithm 
requires  computing  the  inverse  of  a  4x4  matrix,  but  it  is  a 
single-frame  or  noniterative  algorithm.  The  QUEST  algorithm 
needs  to  be  executed  once  for  each  sampling  step  of  the  Kalman 
filter.  The  Gauss-Newton  method  needs  to  be  iteratively  evalu¬ 
ated  several  times  until  it  converges  for  each  sampling  step  of 
the  Kalman  filter. 

VI.  Kalman  Filter  Implementation 

In  this  section,  the  implementation  of  the  second  approach 
Kalman  filter  design  will  be  described.  First,  the  state  equations 
are  linearized  and  discretized  to  yield  a  discrete  process  model. 
Second,  modeling  of  the  process  noises  and  measurement  noises 
is  presented. 

The  state  equations  (2)  and  (3)  can  be  written  together  in  the 
following  form: 

X  =  f(x)+w(t).  (9) 

This  nonlinear  process  model  can  be  linearized  along  the  cur¬ 
rently  estimated  trajectory  x 

Ax  =  ^4^  |W  Ax  +  w(t).  (10) 

OX 

The  actual  trajectory  x  is  the  sum  of  the  estimated  trajectory  x 
and  the  small  increment  Ax 

x  =  x  +  Ax.  (11) 

The  next  step  is  to  convert  the  continuous-time  model  (10)  into  a 
discrete-time  model.  Let  6  =  At  be  the  sampling  interval.  Then 
the  difference  equation  corresponding  to  the  differential  (10)  is 
given  by 

Axk+1  =  $kAxk  +  wk  (12) 

where  the  discrete  state  transition  matrix  is 
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Fig.  4.  Block  diagram  of  the  EKF. 


and  wk  is  a  vector  of  discrete  white  process  noise  and  its  ele¬ 
ments  are  given  by 

{+ 

ftk+1e  -i  w^d-y,  *  =  1,2,3  (14) 

0,  i  =  4,5,6,7. 

The  measurement  (8)  are  linear  and  thus  linearization  is  not 
required.  The  corresponding  discrete  measurement  equation  is 
given  by 


zk  =  Hkxk  +  vk  (15) 

where  Hk  is  the  7x7  identity  matrix.  An  EKF  can  now  be  de¬ 
signed  for  the  discrete  process  (12)  and  the  discrete  measure¬ 
ment  (15).  A  complete  diagram  of  the  filter  is  depicted  in  Fig.  4. 
It  is  seen  from  Fig.  4  that  the  model  parameters  &k ,  Hk ,  Rk ,  and 
Qk  need  to  be  provided  to  start  the  filter.  <frk  is  the  discrete  state 
transition  matrix  given  by  (13).  Hk  is  the  identity  measurement 
equation  matrix  of  (15).  The  determination  of  the  covariance 
matrix  Qk  of  the  process  noises  and  the  covariance  matrix  Rk 
of  the  measurement  noises  is  discussed  below. 

The  process  noise  covariance  matrix  Qk  is  defined  by 


Qk  =  E[wkwl ]  (16) 

where  E  is  the  expectation  operator,  and  wk  is  the  discrete 
process  white  noise  vector  of  (12),  whose  components  are  given 
by  (14).  Before  computing  Qk ,  it  should  be  noted  that  the  con¬ 
tinuous  process  noises  w(t)  =  [wi(t),W2(t),ws(t)]T  of  the 
state  (2)  are  independent  white  noises  with  zero  mean  and  vari¬ 
ance  Di.  As  such,  the  covariance  is  given  by 

E[Wi{t)Wj{s)\  =  {  ^ 8{t  ~s)'  \  =  : J.  (17) 


Fig.  5.  Comparison  of  the  simulated  angular  rate  (left)  and  actual  angular  rate 
measurements  (right). 


Using  (17)  and  (14),  the  process  noise  covariance  matrix  Qk  of 
(16)  is  evaluated  to  be 
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(18) 


where  #n,  <722*  and  Q33  are  given  by 

qii  =  E[wlkwlk]  =  —  (l  -  e  Ti  J  (19) 

q22  =  E[w2kw2k]  =  (l  -  (20) 

r  n  D<1  f  2  At  A 

933  =  E[w 3kW3k]  =  ( 1  -  e  )  .  (21) 


What  remains  to  be  determined  are  the  variance  Di  of  the  con¬ 
tinuous  white  noise  processes  and  the  time  constant  r;  of  the 
process  model.  They  are  determined  using  actual  measurement 
data  from  the  MARG  sensors  and  a  Matlab  simulation  imple¬ 
menting  the  angular  rate  process  model  (2).  The  variance  and 
time  constant  in  the  simulation  are  adjusted  until  the  output  of 
the  simulation  closely  matches  the  actual  measurement  data.  For 
this  purpose,  a  MARG  sensor  was  attached  to  the  right  lower 
arm  of  a  user  and  typical  arm  motion  data  were  collected.  It  was 
experimentally  determined  that  tv;  =  0.5  s,  Di  =  0.4  rad2/s2. 

Fig.  5  shows  a  comparison  between  the  simulated  angular 
rates  and  the  actual  angular  rates  obtained  from  a  MARG  III 
sensor  for  typical  arm  motions.  The  graphs  to  the  left  represent 
the  angular  rates  generated  by  the  simulation  model.  The  graphs 
to  the  right  are  the  angular  rates  measured  by  a  MARG  sensor. 
It  can  be  observed  that  the  two  sets  of  data  exhibit  similar  char¬ 
acteristics.  Autocorrelations  of  the  simulated  and  actual  x-axis 
angular  rate  data  are  plotted  in  Fig.  6.  The  autocorrelation  of  the 
actual  angular  rate  data  obtained  from  the  MARG  sensor  was 
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Autocorrelation  of  the  simulated  (top)  and  actual  (bottom)  angular  rates 


Fig.  6.  Autocorrelations  of  the  simulated  x-axis  angular  rate  (top  plot)  and  the 
actual  x-axis  angular  rate  (bottom  plot). 

first  computed.  The  parameters  of  the  process  model  were  then 
adjusted  so  that  the  autocorrelation  of  the  simulated  angular  rate 
closely  matches  that  of  the  actual  data.  It  is  seen  that  they  are 
not  exactly  the  same,  but  closely  resemble  each  other. 

The  measurement  noise  covariance  matrix  Rk  represents  the 
level  of  confidence  placed  in  the  accuracy  of  the  measurements, 
and  is  given  by 

Rk  =  E[vkVk].  (22) 

In  principle,  Rk  is  not  necessarily  diagonal.  For  practical  pur¬ 
poses,  only  diagonal  elements  are  experimentally  determined 
based  on  actual  measurements.  A  MARG  sensor  was  placed  in 
various  static  configurations,  and  data  were  collected.  The  vari¬ 
ances  of  the  first  three  measurement  components  are  determined 
directly  from  angular  rate  measurements,  and  the  variances  of 
the  other  four  components  (quaternion  components)  are  deter¬ 
mined  using  computed  quaternions.  The  experimentally  deter¬ 
mined  values  are  Rn  =  R22  =  R33  =  0.01  rad2/s-2,  and 
R44  —  R33  =  Rqq  =  R77  =  0.0001. 

VII.  Offline  MATLAB  Testing  Results 

After  deriving  all  the  required  parameters  to  initialize  the 
Kalman  filter,  it  was  implemented  using  MATLAB  to  test  the 
performance  and  accuracy  of  the  quaternion  orientation  esti¬ 
mates.  Real  world  data  recorded  using  a  MARG  sensor  was  used 
in  these  tests. 

Since  the  Kalman  gain  was  determined  such  that  the  sum  of 
squared  errors  is  minimized,  one  way  to  measure  the  conver¬ 
gence  of  the  Kalman  filter  is  through  examination  of  the  trace 
of  the  error  covariance  matrix  Pk .  Fig.  7  shows  the  trace  of  Pk 
for  the  first  200  samples  of  data  obtained  with  the  sensor  in  its 
reference  position  (x-axis  pointing  north,  y-axis  pointing  east, 
and  z-axis  point  down).  It  is  noted  that  the  sum  of  squared  er¬ 
rors  reaches  a  steady  state  after  approximately  0.6  s. 

Table  I  shows  the  elements  of  the  quaternion  for  the  first  five 
samples.  The  initial  estimate  was  chosen  to  be  the  unit  quater- 


Fig.  7.  Trace  of  the  error  covariance  matrix. 


TABLE  I 

Convergence  of  the  Quaternion  Estimates 
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72 
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0.99985 
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0.0066032 

0.013570 

2 

0.99991 

0.0057585 

0.0049037 

0.011901 

3 

0.99990 

0.0055983 

0.0048826 

0.011882 

4 

0.99990 

0.005288 

0.0046884 

0.011784 

5 

0.99990 

0.0052297 

0.0046353 

0.011506 

nion  (0.5,  0.5,  0.5,  0.5).  The  actual  position  of  the  sensor  in  the 
reference  position  is  represented  by  the  quaternion  (1,  0,  0,  0). 
The  data  shown  in  Table  I  indicates  that  the  Kalman  filter  esti¬ 
mate  converged  to  the  actual  position  in  a  single  iteration. 

While  the  QUEST  algorithm  works  well  for  static  orientation 
and  slow  movements,  the  objective  of  the  Kalman  filter  is  to 
blend  angular  rate  measurements  with  the  estimates  produced 
using  magnetometer  and  accelerometer  data  during  periods  in 
which  the  sensor  module  is  subjected  to  motions  involving  high 
angular  rates  and  large  linear  accelerations.  To  verify  the  esti¬ 
mation  accuracy  during  such  periods,  the  orientation  estimates 
of  the  Kalman  filter  were  compared  with  the  estimates  produced 
using  only  the  QUEST  algorithm  with  no  rate  measurement  and 
with  the  reference  motion  of  a  precision  tilt  table.  Two  kinds 
of  experiments  were  conducted  for  this  test.  The  first  used  con¬ 
trolled  rotations  produced  by  a  HAAS  precision  tilt  table.  The 
table  has  two  DOFs  and  is  capable  of  positioning  to  an  accuracy 
of  0.001°  at  rates  ranging  from  0.001  to  80°/s.  In  order  to  mit¬ 
igate  any  possible  magnetic  effects  generated  by  the  steel  con¬ 
struction  of  the  tilt  table,  the  sensor  package  was  mounted  on 
a  nonferrous  extension  above  the  table  as  shown  in  Fig.  8.  The 
extension  is  made  of  a  piece  of  PVC  pipe  and  is  approximately 
1  m  in  length.  The  second  experiment  used  a  random  motion 
pattern  produced  while  the  sensor  was  attached  to  the  arm  of  a 
person. 

In  the  first  set  of  experiments,  the  sensor  was  initially  placed 
with  its  xyz  axes  aligned  with  north-east-down  directions.  The 
sensor  was  rotated  —90°  about  the  x-axis  at  a  rate  of  60°/s  and 
then  rotated  90°  at  the  same  rate  (in  the  reverse  direction)  for 
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Fig.  8.  Experimental  setup  using  a  HAAS  precision  tilt  table. 
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Fig.  10.  Zoom-in  view  of  the  roll  estimate  (solid  curve)  from  the  Kalman  filter 
and  the  tilt  table  reference  motion  (dashed  curve)  with  a  90°  rotation  in  roll. 
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Fig.  9.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the  Fig.  1 1 .  Difference  between  the  roll  estimate  and  the  tilt  table  reference  motion. 
Kalman  filter  (right)  with  a  90°  rotation  in  roll  axis. 


two  cycles.  Fig.  9  shows  the  performance  of  the  Kalman  filter 
in  estimating  the  orientation  of  the  sensor.  The  graphs  to  the 
left  show  the  orientation  estimated  by  the  QUEST  algorithm, 
and  the  graphs  to  the  right  show  the  orientation  estimated  by 
the  Kalman  filter.  It  can  be  seen  that  the  QUEST  algorithm  was 
able  to  correctly  estimate  the  roll  angle  before  the  first  (negative) 
rotation,  between  the  first  and  second  (positive)  rotations,  and 
after  the  second  rotation,  but  it  is  not  able  to  correctly  estimate 
orientation  during  the  rotational  motions.  During  the  rotational 
motions,  the  accelerometers  measure  the  sum  of  gravity  and  mo¬ 
tion  induced  acceleration.  Without  rate  sensors,  the  QUEST  al¬ 
gorithm  is  not  able  to  differentiate  gravity  from  the  motion  ac¬ 
celeration.  Relatively  large  errors  in  pitch  and  yaw  were  also 
produced  by  the  QUEST  algorithm.  On  the  other  hand,  it  can 
be  seen  from  the  top-right  plot  that  the  Kalman  filter  was  able 
to  correctly  estimate  the  roll  angle  throughout  the  duration  of 
the  experiment.  The  small  pitch  and  yaw  motions  seen  in  the 
center-right  and  bottom-right  plots  are  due  to  misalignment  of 


the  sensor  module  with  the  motion  axes  of  the  experimental  tilt 
table.  The  misalignments  were  corrected  manually,  but  could 
not  be  completely  removed  without  the  use  of  equipment  not 
available  to  the  authors.  To  confirm  that  the  errors  were  due 
to  misalignments,  the  algorithm  was  tested  using  synthetically 
generated,  noise-free  data  with  rotation  in  only  one  axis.  These 
results  demonstrated  that  the  algorithm  does  not  produce  any 
observable  cross-coupling  responses  in  other  axes. 

To  illustrate  the  accuracy  of  the  Kalman  filter,  the  estimates 
produced  by  the  Kalman  filter  can  be  compared  with  the  mo¬ 
tion  of  the  tilt  table.  Since  the  tilt  table  used  in  the  experiments 
is  much  more  accurate  than  the  tracking  system  under  evalua¬ 
tion,  its  motion  can  be  treated  as  a  truth  reference.  In  Fig.  10, 
the  top-right  plot  of  Fig.  9  is  replotted  in  a  zoom-in  view  for  the 
time  period  of  7-15  s.  The  solid  curve  represents  the  roll  esti¬ 
mate  from  the  Kalman  filter,  and  the  dashed  curve  is  the  refer¬ 
ence  trajectory  of  the  tilt  table.  The  difference  between  these  two 
curves  is  shown  in  Fig.  11.  It  is  observed  from  Figs.  10  and  11 
that  the  static  accuracy  of  the  filter  is  better  than  2°  for  the  time 
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Fig.  12.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the 
Kalman  filter  (right)  with  a  45°  rotation  in  pitch  axis. 


Fig.  14.  Snapshot  of  real-time  testing.  The  user  with  two  MARG  sensors  at¬ 
tached  to  the  right  arm  is  the  foreground  and  the  human  avatar  projected  on  a 
screen  is  in  background. 
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Fig.  13.  Orientation  estimate  produced  by  the  QUEST  algorithm  (left)  and  the 
Kalman  filter  (right)  with  random  arm  movements. 


Fig.  15.  Another  snapshot  of  real-time  testing. 


periods  of  about  7-9.8  s  and  11.3-15  s.  During  the  time  period 
of  9.8-1 1 .3  s,  the  tilt  table  and  the  MARG  sensor  are  in  the  dy¬ 
namic  state  moving  from  —90.0°  to  0.0°  at  the  rate  of  60°/s.  It 
can  be  observed  from  Fig.  1 1  that  the  maximum  error  is  about  9°. 
This  large  dynamic  error  is  mainly  due  to  the  lag  of  the  tracking 
system.  The  lag  is  on  the  order  of  100  ms,  as  depicted  by  the 
horizontal  gap  between  the  blue  curve  and  green  curve  during 
the  time  period  of  10-1 1  s.  The  sampling  rate  is  100  Hz,  which 
yields  a  lag  of  10  ms.  The  computational  time  required  to  ex¬ 
ecute  the  filter  algorithm  is  about  1.6  ms.  The  additional  lag  is 
caused  by  data  transmission.  In  human  body  tracking  applica¬ 
tions,  this  lag-induced  error  is  only  observable  during  highly 
dynamic  motion,  and  is  not  of  great  enough  magnitude  to  im¬ 
pair  user  interaction  with  a  virtual  environment. 

Fig.  12  shows  plots  of  rotating  the  sensor  about  the  y-axis  first 
by  45°  and  then  by  —45°  at  a  rate  of  45°/s.  Similar  results  are 
observed  in  this  experiment. 

Fig.  13  shows  the  results  of  an  experiment  in  which  the  sensor 
was  rotated  randomly  while  attached  to  the  arm  of  a  person.  Al¬ 


though  there  is  no  true  reference  in  this  case,  it  can  be  seen  that 
the  Kalman  filter  eliminated  the  jittering  and  spiking  contained 
in  the  orientation  estimates  produced  by  using  the  QUEST  al¬ 
gorithm  alone. 

VIII.  Real-Time  Testing  Results 

After  initial  testing  of  the  EKF  with  the  MATLAB  implemen¬ 
tation,  the  QUEST  algorithm  and  EKF  algorithm  were  imple¬ 
mented  in  Java  for  real-time  testing  and  evaluation.  Computa¬ 
tion  time  required  to  perform  a  single  update  is  1 .6  ms.  Memory 
management  in  the  Java  implementation  is  carefully  performed 
to  avoid  the  requirement  for  garbage  collection  and  possible 
interruption  of  filter  processing.  The  real-time  quaternion  pro¬ 
duced  by  the  Kalman  filter  was  visualized  using  a  human-like 
avatar  as  seen  in  Figs.  14  and  15.  Two  MARG  sensors  were  used 
to  track  the  motion  of  a  human  arm,  one  sensor  being  attached  to 
the  upper  arm  and  the  other  attached  to  the  lower  arm.  A  video 
clip  demonstrating  real- tracking  of  human  arm  motions  is  avail¬ 
able  at  http://ieeexplore.ieee.org. 
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The  QUEST  algorithm  was  able  to  track  the  motion  of  the 
human  arm  under  slow-moving  conditions  where  linear  accel¬ 
eration  was  not  significant.  However,  when  the  arm  motion  be¬ 
came  faster,  the  algorithm  was  not  able  to  follow  the  arm  motion, 
resulting  in  observable  lag  as  well  as  overshoots. 

When  the  EKF  was  integrated  with  the  QUEST  algorithm,  the 
avatar  was  able  to  successfully  track  the  human  arm  motion  in 
real  time  under  all  conditions.  Furthermore,  the  filtering  process 
did  not  produce  any  noticeable  lag.  Movement  of  the  human  arm 
and  the  avatar  was  synchronized. 

IX.  Conclusion 

This  paper  presents  the  design,  implementation,  and  experi¬ 
mental  results  of  a  quaternion-based  Kalman  filter  for  real-time 
human  body  motion  tracking  using  inertial/magnetic  sensor 
modules  containing  orthogonally  mounted  triads  of  accelerom¬ 
eters,  angular  rate  sensors,  and  magnetometers.  This  subject 
filter  is  not  applicable  to  applications  in  which  accelerations 
due  to  forces  other  than  gravity  are  present  for  indefinite  pe¬ 
riods.  The  filter  was  designed  with  the  goal  of  being  able  to 
produce  highly  accurate  orientation  estimates  in  real  time.  This 
real-time  requirement  precluded  the  use  of  complex  models 
of  human  motion.  Instead  the  filter  design  makes  use  of  a 
simple  first-order  linear  system  model.  Output  of  the  model 
is  angular  velocity  modeled  as  colored  noise  generated  from 
white  noise  input.  The  Kalman  filter  design  is  further  simplified 
by  preprocessing  accelerometer  and  magnetometer  data  using 
the  single-frame  QUEST  algorithm.  The  quaternion  produced 
by  QUEST  is  provided  as  input  to  the  Kalman  filter  along  with 
angular  rate  data.  In  comparison  to  more  traditional  approaches, 
this  preprocessing  step  significantly  reduces  the  complexity 
of  filter  design  by  allowing  the  use  of  linear  measurement 
equations.  Prior  to  testing  of  the  filter  algorithm,  values  for 
variances  and  time  constants  where  determined  by  comparing 
simulation  results  to  actual  measurement  data  obtained  during 
typical  arm  motions.  This  process  was  considered  complete 
when  the  autocorrelation  of  the  simulation  data  closely  matched 
that  of  the  actual  data.  In  experiments  designed  to  validate  filter 
performance,  this  approach  was  shown  to  work  well.  In  these 
experiments,  filter  orientation  estimates  were  compared  with 
truth  data  obtained  from  a  rotary  tilt  table.  Filter  response  very 
closely  matched  tilt  table  motion  with  a  static  accuracy  better 
than  2°  and  a  dynamic  accuracy  of  better  than  9°.  This  larger 
error  during  motion  was  largely  caused  by  data  communication 
delays.  Even  with  this  delay,  qualitative  experiments  in  which 
the  algorithm  was  used  demonstrate  that  these  dynamic  errors 
were  not  of  great  enough  magnitude  to  impair  user  interaction 
with  a  virtual  environment. 

The  Kalman  filter  design  presented  in  this  paper  is  the  result 
of  several  years  of  effort.  With  refinement  of  this  design  and 
others  mentioned  in  the  related  work  section,  orientation  esti¬ 
mation  algorithms  have  reached  a  limit  given  the  accuracy  and 
noise  characteristics  of  the  MEMs  sensors  employed  in  the  ap¬ 
plication.  The  angular  rate  sensors  and  accelerometers  are  truly 
“sourceless”  and  do  not  depend  on  any  outside  reference.  How¬ 
ever,  though  it  is  not  artificially  generated,  the  magnetometers 
must  sense  a  homogenous  ambient  magnetic  field  in  order  for 
these  systems  to  deliver  orientation  estimates  that  are  stable  in 
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azimuth.  Thus  the  ultimate  accuracy  of  these  algorithms  can  not 
be  determined  by  considering  only  the  sensors  and  the  imple¬ 
mented  algorithms. 
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ABSTRACT:  In  human  posture  tracking  applications ,  limb  segment  attitude  can  be  estimated  without  the  aid  of 
a  generated  source  using  small  inexpensive  inertial  / magnetic  sensor  modules.  In  the  absence  of  an  adequate 
process  model  and  process  noise  characteristics  or  in  an  application  in  which  the  dynamic  and  measurement  rela¬ 
tions  are  non-linear,  a  simple  complementary  filter  represents  a  computationally  inexpensive  solution  that  pro¬ 
duces  accurate  attitude  estimates  superior  to  those  of  an  improperly  tuned  Kalman  filter:  This  paper  presents  the 
theory  and  experimental  7'esults  for  a  single  parameter  tunable  quaternion  based  attitude  estimation  filter ;  The 
described  sub-optimal  constant  gain  complementary  filter  is  the  first  filter  designed  for  inertial  l  magnetic  sensor 
modules  capable  of  estimating  orientation  in  all  attitudes  without  singularities  while  continuously  connecting  for 
drift  without  the  need  for  still  periods.  Experimental  results  qualitatively  and  quantitatively  demonstrate  the  accu¬ 
racy  and  stability  of  the  filtering  algorithm. 


INTRODUCTION 

Accurate  real-time  tracking  of  the  orientation  or 
attitude  of  rigid  bodies  has  wide  application  in  nav¬ 
igation,  robotics  [1],  helicopters  [2],  tele-operation, 
augmented  reality,  and  virtual  reality  [3],  Advances 
in  the  field  of  miniature  sensors  have  made  possible 
inertial/magnetic  tracking  of  the  three  degrees  of 
orientation  using  sensor  modules  containing  three 
orthogonal  angular  rate  sensors,  three  orthogonal 
linear  accelerometers  and  three  orthogonal  magne¬ 
tometers.  The  orientation  estimates  are  based  on 
the  passive  measurement  of  physical  quantities  that 
are  directly  related  to  the  rate  of  rotation  and  the 
orientation  of  the  tracked  rigid-body.  The  estimates 
are  expressed  relative  to  an  Earth  fixed  reference 
frame. 

The  purpose  of  the  human  body  tracking  system 
is  to  estimate  the  orientation  of  multiple  human 
limb  segments  and  use  the  resulting  estimates  to 
set  the  posture  of  the  human  body  model  that  is 
visually  displayed.  Figure  1  depicts  experiments 
designed  to  qualitatively  evaluate  and  demonstrate 
this  capability.  In  these  experiments  three  inertial/ 
magnetic  sensor  modules  attached  to  the  limb  seg- 
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ments  to  he  tracked.  Video  recordings  of  the  system 
in  normal  operation  indicate  that  posture  estima¬ 
tion  was  accurate  and  showed  very  little  lag.  A 
quicktime  movie  of  these  experiments  may  be  down¬ 
loaded  at  http://www.users.muohio.edu/bachmaer/ 
research.htm.  This  video  qualitatively  demonstrates 
the  dynamic  accuracy  of  the  quaternion  attitude 
estimation  filter  which  is  the  subject  of  this  article. 

A  naive  approach  to  inertial  orientation  tracking 
might  simply  involve  integration  of  angular  rate 
data  to  determine  orientation.  However,  this  solu¬ 
tion  is  prone  to  drift:  over  time  due  to  the  buildup  of 
small  bias  and  drift  errors.  In  order  to  avoid  drift, 
inertial  tracking  systems  make  use  of  complemen¬ 
tary  sensors  to  continuously  correct  the  orientation 
estimate.  Accelerometers  are  used  for  low  frequency 
measurement  of  the  gravity  vector  relative  to  the 
coordinate  frame  of  the  sensor  module.  Since 
accelerometers  actually  sense  the  sum  of  gravity 
and  linear  acceleration  due  to  motion,  low  pass  fil¬ 
tering  is  generally  required  to  discriminate  against 
the  latter.  Magnetometers  serve  a  similar  function 
for  the  local  magnetic  field  vector.  Taken  together, 
the  use  of  accelerometers  and  magnetometers 
and  the  use  of  angular  rate  sensors,  respectively 
represent  complementary  low  and  high  frequency 
methods  of  attitude  estimation.  When  combined, 
these  two  types  can  also  be  used  for  continuously 
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Fig.  1- Inertial  I  Magnetic  Ducking  of  the  Arm  and  Leg  Using  Three  Sensor  Modules  [4] 


accurate  attitude  estimation  in  dynamic  applica¬ 
tions  without  latency. 

In  recent  years,  this  type  of  problem  has  most 
commonly  been  solved  by  designing  a  Kalman  filter 
to  integrate  the  data  froru  the  complementary 
sensor  types.  In  applications  such  as  spacecraft  atti¬ 
tude  estimation  where  the  dynamics  of  the  tracked 
body  are  well  defined  and  an  accurate  process  model 
is  available,  the  ideal  solution  to  the  tracking  prob¬ 
lem  would  be  an  extended  Kalman  filter  [5].  In  the 
work  described  here,  the  targeted  application  is 
real-time  human  posture  tracking.  Dynamic  models 
for  the  musculoskeletal  system  have  been  studied 
for  many  years  [6].  Such  models  are  ideal  for  com¬ 
puter  simulations  of  articulated  body  motions,  but 
remain  too  computationally  demanding  for  real¬ 
time  applications  such  as  human  motion  tracking. 
Thus,  the  challenge  would  be  to  develop  a  model 
that  is  adequate,  but  not  overwhelmingly  complex 
for  motion  tracking  applications.  In  the  end,  how¬ 
ever,  it  may  be  the  case  that  a  properly  tuned  com¬ 
plementary  filter  will  provide  attitude  estimates 
with  an  accuracy  that  '  is  comparable  to  those 
made  by  an  extended  Kalman  filter  without  the 
associated  complexity  and  development  time.  Thus, 
the  prototype  research  described  here  makes  use  of 
a  complementary  filter  based  upon  a  quaternion 
representation  of  orientation. 

The  remainder  of  this  paper  will  describe  a  proto¬ 
type  attitude  estimation  filter  designed  for  an  iner¬ 
tial/magnetic  human  body  tracking  system.  The 
filter  processes  sensor  data  and  produces  orienta¬ 
tion  estimates  expressed  in  quaternion  form  [7] 
without  singularities.  Estimation  error  is  minimized 
using  Gauss-Newton  iteration.  Unlike  filters  previ¬ 
ously  designed  for  the  targeted  application,  drift  is 
corrected  continuously  without  any  requirement  for 
still  periods.  Experimental  results  qualitatively  and 
quantitatively  demonstrate  the  performance  of  the 
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filter  showing  that  the  produced  attitude  estimates 
are  stable  and  accurate.  The  work  described  here 
laid  the  foundation  for  numerous  attitude  estima¬ 
tion  filters  designed  for  inertial/magnetic  sensor 
modules  [8,  9], 

Background  and  Related  Work 

Use  of  angular  rate  sensors  and  accelerometers, 
or  inertial  measurement  units  (IMU)  in  land  and/or 
underwater  robots  has  been  well  documented 
[1]  [10  - 12] .  A  study  of  human  motion  tracking  using 
accelerometers  alone  was  reported  in  [11],  For 
motion  involving  small  linear  accelerations,  a  set 
of  tri-axial  accelerometers  was  used  to  determine 
two  degree  of  freedom  (DOF)  rotation  angles. 
During  motions  accompanied  by  higher  accelera¬ 
tions,  a  technique  is  described  that  involves  the  use 
of  two  sets  of  tri-axial  accelerometers  on  a  single 
rigid-body  to  differentiate  gravitational  acceleration 
from  motion  related  linear  acceleration.  Reference 
[14]  described  an  attitude  estimation  algorithm 
based  on  the  use  of  angular  rate  sensors  and 
accelerometers.  In  this  case,  drift  in  heading  esti¬ 
mation  was  unavoidable  due  to  a  lack  of  additional 
complementary  sensors  such  as  magnetometers. 

Reference  [15]  presents  an  attitude  tracking  sys¬ 
tem  with  GPS  and  inertial  sensors  used  for  aircraft. 
Differences  between  the  GPS  signals  received  by 
three  antennas  are  used  to  estimate  attitude. 
Reference  [16]  replaces  the  antenna  information 
with  celestial  observation  data.  Reference  [17] 
describes  an  attitude  package,  which  combines  the 
outputs  of  inclinometers,  gyros,  and  compasses  to 
obtain  attitude  estimation.  All  three  examples  utilize 
Euler  angles  to  represent  orientation  and  a  Kalman 
filtering  algorithm  to  integrate  the  information. 

Reference  [18]  presented  the  first  inertial/ 
magnetic  system  for  head  tracking  applications. 
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This  system  utilized  a  fluid  pendulum  and  three 
solid-state  piezoelectric  angular  rate  sensors.  The 
initial  system  did  not  include  a  compass  or  magne¬ 
tometers  and  thus  drifted  about  the  vertical  axis. 
Subsequent  systems  include  three  orthogonal  solid- 
state  rate  sensors,  a  two-axis  fluid  inclinometer  and 
a  two-axis  fluxgate  compass  [19].  InterSense,  Inc, 
was  started  as  a  result  of  this  research  and  contin¬ 
ues  to  produce  inertial  tracking  devices  designed  for 
head  tracking  applications.  Sensor  data  is  processed 
by  a  complementary  separate-bias  Kalman  filter 
which  required  periods  of  “still  time55  to  correct  for 
rate  sensor  drift  [19]. 

The  fact  that  inertial  data  lends  itself  to  predic¬ 
tion  through  the  use  of  motion  derivatives  has 
resulted  in  the  use  of  inertial  sensors  in  numerous 
efforts  to  combat  latency  problems.  Reference  [20] 
demonstrates  that  predicting  future  head  location 
using  three  angular  rate  sensors  and  three  linear 
accelerometers  is  an  effective  approach  for  signifi¬ 
cantly  reducing  dynamic  errors  in  an  augmented 
reality  head  tracking  system.  In  this  study  predic¬ 
tion  caused  the  head  tracking  dynamic  accuracy  to 
increase  by  factors  of  5  to  10.  Linear  Kalman  filters 
are  used  to  estimate  and  predict  translation  terms 
and  an  Extended  Kalman  Filter  (EKF)  is  used  to 
estimate  and  predict  orientation  terms. 

Estimating  Orientation 

This  section  describes  how  angular  rate,  gravita¬ 
tional  acceleration,  and  the  local  magnetic  field 
sensed  in  the  coordinate  frame  of  a  moving  rigid- 
body  can  be  used  to  estimate  the  orientation  of  the 
body  relative  to  an  Earth  fixed  reference  frame.  For 
illustrative  purposes,  the  discussion  begins  with  a 
static  situation  in  which  the  body  is  not  in  motion 
and  later  describes  how  the  same  techniques  are 
used  in  a  dynamic  tracking  situation.  All  rotations 
are  expressed  using  quaternions.  The  use  of  quater¬ 
nions  reduces  the  number  of  required  scalar  oper¬ 
ations,  and  eliminates  calculations  involving 
trigometric  functions.  In  addition,  using  quater¬ 
nions  allows  the  avoidance  of  singularities  associ¬ 
ated  with  Euler  angle  descriptions  of  orientation. 

Estimation  Using  Accelerometers 
and  Magnetometers 

Accelerometers  measure  the  combination  of 
forced  linear  acceleration  and  the  reaction  force  due 
to  gravity.  Thus,  the  output  of  an  accelerometer 
triad  is  given  by 

^measured 

For  bodies  that  are  not  in  motion,  forced  linear 
acceleration  is  zero  (a  ~  0).  In  this  case  accelerome¬ 
ters  sense  only  gravity.  Gravity  in  Earth  coordinates 


is  always  down  and  can  be  expressed  as  the  down 
unit  vector  in  quaternion  form  as 

Em  =  [0  0  0  -1]  (2) 

where  the  superscript,  E,  indicates  the  vector  is 
described  relative  to  an  Earth  fixed  reference  frame. 
Three  orthogonally  mounted  accelerometers  sense 
the  same  vector  relative  to  their  own  coordinate 
frame.  This  vector  quantity  can  also  be  expressed  as 
a  pure  vector  quaternion 

sh  =  [0  h2  h2  h3]  (3) 

where  the  superscript,  S,  indicates  that  the  vector  is 
described  relative  to  the  moving  coordinate  frame  of 
the  sensor  module.  Both  m.  and  h  describe  the  same 
vector.  If  it  is  assumed  that  there  is  no  measurement 
error  in  the  accelerometers,  all  differences  can  be 
attributed  to  rotation  of  the  frame  of  reference  of  the 
module.  This  rotation  can  be  expressed  using 
quaternion  multiplication  involving  a  quaternion, 
q,as:  ’ 

sh  =  q”1  ®  Em  ®  q  (4) 

Solving  for  q  based  on  the  known  m  and  the  meas¬ 
ured  h  produces  an  expression  for  the  orientation  of 
the  sensor  relative  to  the  Earth  fixed  reference 
frame.  Unfortunately,  the  quaternion,  q,  will  remain 
constant  if  the  body  is  rotated  about  a  vertical  axis 
aligned  with  the  gravity  vector.  To  eliminate  this 
ambiguity,  another  vector  that  is  assumed  to  remain 
constant  over  a  given  working  volume  is  used  as  a 
reference. 

Like  gravity,  the  direction  of  the  local  magnetic 
field  is  a  known  quantity.  It  can  be  expressed  rela¬ 
tive  to  the  Earth  fixed  reference  frame  as  the  pure 
vector  quaternion 

En  =  [0  n2  n3]  (5) 

Three  orthogonally  mounted  magnetometers  can 
sense  the  same  local  magnetic  field  relative  to  their 
own  coordinate  frame.  The  pure  vector  quaternion, 
b,  is  used  to  express  this  sensor  based  quantity. 

sb  =  [0  bx  b2  b8]  (6) 

if  it  is  assumed  that  the  axes  of  the  accelerometer 
triad  are  aligned  with  those  of  the  three  axis  mag¬ 
netometer.  Both  the  gravity  and  magnetic  field 
vectors  are  measured  relative  to  the  same  sensor 
coordinate  frame.  If  the  magnetometers  perfectly 
measure  the  three  components  of  the  local  magnetic 
field  vector,  the  quaternion,  q,  from  Eq.  (4)  that 
relates  the  rotated  frame  of  the  accelerometers  to 
the  Earth  fixed  reference  frame  also  relates  the 
rotated  frame  of  the  magnetometers  to  the  Earth 
fixed  reference  frame.  That  is, 

sb  -  q'1©  En®  q  (7) 
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Applying  the  rationale  of  Eq.  (4),  Eq.  (7)  will  remain 
constant  if  the  tracked  body  is  rotated  about  an  axis 
aligned  with  the  local  magnetic  field  vector. 
However,  under  the  assumption  that  the  magnetic 
and  gravity  vectors  are  not  parallel,  both  equations 
can  be  used  together  to  determine  orientation.  Any 
axis-aligned  rotations  that  allow  one  equation  to 
remain  constant  will  result  in  a  change  in  the  meas¬ 
ured  quantities  involved  in  the  other.  As  shown  later 
in  this  paper,  this  circumstance  can  be  used  to 
obtain  a  unique  estimated  orientation  quaternion, 
q.  Solving  for  q  based  on  measurements  of  the 
gravity  and  magnetic  field  reference  vectors  is 
essentially  a  quaternion  version  of  the  Wahba 
problem  [21,  22], 

In  order  to  track  the  orientation  of  moving  bodies 
using  the  above  methods,  the  following  assumption 
is  made.  For  all  moving  bodies,  linear  acceleration 
will  average  to  zero  given  a  period  of  sufficient 
length.  Otherwise,  the  bodies  would  soon  reach 
excessive  velocities.  Therefore,  when  averaged  over 
time,  an  accelerometer  triad  returns  only  the  com¬ 
ponents  of  the  gravity  vector  or  the  local  vertical. 
Tliis  averaging  allows  accelerometers  combined 
with  magnetometers  to  be  used  as  a  low  frequency 
source  of  orientation  estimates.  The  length  of  the 
period  over  which  acceleration  must  be  averaged  is 
dependent  upon  the  type  and  frequency  of  motion 
being  tracked.  These  estimates  would  not  be  useful 
in  applications  involving  feedback  control  since  they 
would  tend  to  lag  behind  the  true  orientation  of  an 
object  undergoing  rapid  changes  in  orientation. 
However,  in  experiments  they  have  been  found  to 
still  be  useful  by  themselves  in  applications  where 
some  lag  is  acceptable  and  it  is  desirable  to  keep  the 
cost  of  individual  sensor  modules  to  a  minimum. 


Estimation  Using  Angular  Rate  Sensors 

Three  orthogonally  mounted  angular  rate  sensors 
can  be  used  to  determine  the  rotational  velocity  of 
a  body  in  three  dimensions.  In  the  same  way  a 
car  speedometer  can  be  used  to  determine  travel 
distance  given  a  known  starting  point,  angular  rate 
sensors  can  be  used  to  determine  attitude.  The 
three-axis  angular  rate  sensor  outputs  measure 
the  rotational  rates  about  the  body  x,  y,  and  z  axes. 
The  measurements  can  be  used  to  construct  a  rate 
vector  that  can  be  expressed  as  the  pure  quaternion 

sco  =  [0  p  q  r]  (8) 

where  p,  q,  and  r,  respectively,  represent  the  rates  of 
rotation  about  the  body  x,  y,  and  z  axes.  The  pure 
quaternion,  is  used  to  obtain  a  rate  quaternion 
[23], 


0) 


where  q  is  a  quaternion  representing  the  current 
orientation,  the  indicated  product  is  a  quaternion 
product,  and  q  is  expressed  relative  to  the  Earth 
fixed  reference  frame. 

If  the  sensor  data  is  noiseless  and  unbiased,  the 
rate  quaternion  derived  in  Eq.  (9)  can  be  integrated 
for  an  unlimited  amount  of  time  to  determine  the 
orientation  of  the  rigid-body  to  which  the  sensors 
are  attached.  In  reality,  low-cost  miniature  rate  sen¬ 
sors  can  be  assumed  to  operate  under  some  bias  and 
noise  effects.  In  the  same  way  distance  estimates 
made  using  a  poorly  calibrated  speedometer  would 
steadily  become  more  and  more  inaccurate,  so 
too  will  orientation  estimates  based  on  the  use  of 
miniature  angular  rate  sensors.  Thus,  the  angular 
rate  sensors  can  only  be  considered  a  high  frequency 
source  of  orientation  estimates  that  must  be  peri¬ 
odically  or  continuously  corrected  using  a  low 
frequency  source. 

Quaternion  Based  Complementary 
Attitude  Filter 

The  two  previous  sections  establish  the  comple¬ 
mentary  nature  of  orientation  estimates  derived 
using  accelerometers  and  magnetometers  and  those 
derived  using  angular  rate  sensors.  An  efficient 
quaternion  based  complementary  attitude  filter 
has  been  devised  that  combines  the  data  from  all 
three  types  of  sensors  to  produce  an  estimate  that  is 
continuously  accurate  through  all  orientations. 
Figure  2  is  a  block  diagram  of  a  simple  complemen¬ 
tary  quaternion-based  attitude  estimation  filter. 
The  filter  inputs  are  a  three-axis  angular  rate 
sensor,  a  three-axis  accelerometer,  and  a  three-axis 
magnetometer.  Its  output  is  a  quaternion  represen¬ 
tation  of  the  orientation  of  the  sensor  module  rela¬ 
tive  to  an  Earth  fixed  reference  frame. 

Parameter  Optimization 

Combining  the  complementary  sensor  inputs  is 
regarded  as  a  parameter  optimization  problem.  The 


Fig,  2-Quaternion  Based  Attitude  Estimation  Filter  14] 
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goal  of  this  optimization  problem  is  to  minimize 
a  modeling  error  that  is  based  on  the  difference 
between  what  the  accelerometers  and  magnetome- 
ters  measure  and  what  they  are  expected  to  mea¬ 
sure  given  the  last  estimated  orientation.  The  rate 
sensors  serve  to  quicken  the  overall  response  of  the 
system. 

The  three  orthogonally  mounted  accelerometers 
return  an  approximation  to  the  local  vertical  rela¬ 
tive  to  the  sensor  coordinate  frame,  the  unit  vector 
h.  The  magnetometer  returns  the  direction  of  the 
local  magnetic  field  relative  to  the  sensor  coordinate 
frame,  the  unit  vector  b.  We  combine  the  vector 
parts  of  the  quaternions  defined  by  Eq.  (3)  and  Eq, 
(6)  to  produce  a  6  X  1  measurement  vector 

T 

y0  “  [hi  h2  h3  bi  b2  b3]  (10) 

In  order  to  compare  the  expected  measurement 
with  what  is  contained  in  the  measurement  vector, 
all  vectors  must  be  expressed  relative  to  the  same 
coordinate  frame.  The  vectors  defined  by  Eq.  (2)  and 
Eq.  (5)  are  mapped  from  the  Earth  fixed  frame  to 
the  sensor  frame  through  quaternion  multiplica¬ 
tions  as  follows  [1] 

sli  =  q“1®Eni®q,  sb  =  q“x®Eb®q  (11) 

Combining  the  vector  parts  of  Eq.  (11)  into  a  single 
6X1  computed  measurement  vector  produces 

y(q)  =  [yi  y2  ys  Ya  ys  y6]T  (12) 

where  y^  y2,  and  ys,  are  the  elements  of  the  vector 
part  of  9h  and  y4,  y5,  and  y6,  are  the  elements  of  the 
vector  part  of  sb.  The  difference  between  the  actual 
measurements  and  the  computed  measurements  is 
the  6x1  modeling  error  vector 

e(q)  -  Yo  “  y(q)  d3) 

In  viewing  Eq.  (13),  note  that  if  there  is  no  meas¬ 
urement  noise  (perfect  sensors),  the  difference 
between  the  measured  and  computed  values  will 
equal  the  zero  vector.  However,  when  real  sensors 
are  used,  this  can  no  longer  be  expected.  Instead,  a 
value  for  the  estimated  orientation  quaternion,  q, 
can  he  obtained  by  minimizing  the  squared  error  cri¬ 
terion  function: 

<p(q)  =  eT(q)e(q)  (14) 

In  the  current  version  of  the  filter,  the  criterion 
function  is  minimized  using  Gauss-Newton  itera¬ 
tion  [24],  This  method  is  based  on  linearized  least 
squares  regression  analysis  where  y0  is  considered  a 
vector  of  data  points  and  y(q)  is  a  vector  to  he  fitted 
to  those  points.  The  full  correction  step  is  [24] 

Aqftjj  =  [XTX]”1XTe(4)  (15). 

where  ^  is  previous  estimate  for  q  and  the 
individual  elements  of  the  measurement  lineariza¬ 
tion  matrix, ,  X,  are  defined  as  [25] 


*-[■£■]  tt6) 

Since  we  are  dealing  with  data  corrupted  by  noise, 
the  correction  step  is  scaled  by  a  value  a  producing 

Aq  partial  =  ^X]'Ve(q)  (17) 

where 

a  =  lcAt  (18) 

and  k  represents  the  filter  gain  value.  Thus,  for  dis¬ 
crete  time  step  integration,  each  successive  estimate 
of  orientation  would  be  calculated  as 

4n+i  =  qn  +  yqns«At  +  oc[XT  X]-1XTe(qn) 

=  4n  +  4  At  +  kAqMi  (19) 

If  ^  is  constrained  to  unit  length  as  depicted  in 
Figure  2,  a  unique  solution  to  the  optimization  prob¬ 
lem  will  exist  and  the  regression  matrix 

S=XrX  (20) 

can  he  inverted  [25],  A  more  efficient  computation  of 
Aq  results  from  noting  that  if 

Qnew  qdd  “1  Aq^u  (21) 

and  if  both  qQid  and  qnew  are  unit  quaternions,  then 
any  small  Attain  must  be  orthogonal  to  q.  If  p  and  q 
are  any  two  quaternions,  then  p  is  orthogonal  to  q  if 
and  only  if  p  is  the  quaternion  product  of  q  and 
a  unique  vector  v  (real  part  equal  to  zero)  where  v  is 
given  by 

v  =  q-1  <2>  p  (22) 

Accordingly,  Aq  can  be  written  in  the  form 

Aq  =  q  ®  v  =  q  ®  [0  vx  v2  v3]  (23) 

With  this  constraint,  linearization  of  the  computed 
measurement  vector,  y(q),  in  Figure  2,  yields 

y(q  +  Aq)  =  y(q)  +  XAq 

=  y(q)  +  X(q  ®  [0  vx  v2  v3])  (24) 

and  consequently: 

— =  X(q  ®  [0  1  0  0])  =  X(q®i)T  (25) 
9vi  • 

-^-  =  X(q®[0  0.  1  0])  =  X(q®j)T  (26) 
Sv2 

=  X(q  ®  [0  0  0  1])  =  X(q  ®  k)T  (27) 
5v3 

where  i,  j,  and  k  are  quaternion  forms  of  unit  vec¬ 
tors  pointing  in  the  directions  of  the  positive  x,  y, 
and  z  axes,  respectively.  Thus,  when  Gauss-Newton 
iteration  is  applied  to  unit  quaternions,  it  is  suffi¬ 
cient  to  solve  for  only  three  unknowns  rather  than 
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four  as  in  the  methods  for  estimation  of  Aqm  in  Eq. 
(15),  That  is,  if  X  is  the  6X3  matrix 


then, 


and 


Xv  = 


f  ay 

3y 

’ 

LflV! 

9v2 

3va . 

AVfun  =  [X^X]~1Xvr(q) 


(28) 

(29) 


Aqmu  =  q  ®  [0,  AvfuU]  (30) 

Incorporation  of  the  above  into  the  Gauss-Newton 
algorithm  notably  simplifies  the  computation  of  the 
Aq  quaternion  since  it  requires  only  a  3  X  3  matrix 
inversion  rather  than  the  4  X  4  matrix  inversion  of 
the  basic  algorithm.  This  is  important  since  best 
algorithms  for  matrix  inversion  are  of  0(n3)  com¬ 
plexity  [26]. 

The  above  dimension  reduction  also  leads  to  an 
efficient  approach  to  the  3X3  problem  formulation. 
Specifically,  let  qr  be  the  incremental  rotation 
quaternion  given  by: 


qr=[l  r2  r3]=[0,r]  (31) 

Evidently,  as  the  rotation  vector,  r,  approaches  the 
zero  vector,  then  qr  approaches  a  unit  quaternion. 
Thus,  if  a  value  is  found  for  qr  that  reduces  the 
squared  error  criterion  function,  ip(q),  it  follows 
that 


qnew  =  qoid®qr  (32) 

and  that,  in  the  limit,  as  r  approaches  zero,  qnew  will 
also  be  a  unit  quaternion.  More  practically,  the 
results  of  Eq.  (32)  should  be  normalized  to  a  unit 
quaternion  after  every  iterative  application  of  this 
equation  [24] ,  thereby  removing  any  limitations  on 
the  magnitude  of  r. 

Eq.  (21)  can  be  rewritten  in  additive  form  by  not¬ 
ing  that 

Aq  ~  qrew  —  qD)d  ~  q0id  ®  [1  ri.  r2  r3] 

-  qoM «  [X  0  0  0]  (33) 
=  qoid®[l  ra  r2  r3]  =  qold®  [0,r]  (34) 

where 

rT=[x^Xv]_1X^E(qold)  (35) 

and,  the  measurement  linearization  matrix,  X,  is 
the  6X3  reduced  order  matrix: 


0 

“ya 

ya 

ya 

0 

-yi 

yi 

0 

0 

~ye 

ye 

ye 

0 

-y4 

-ye 

y4 

0 

(36) 


To  understand  the  importance  of  these  results,  it 
must  be  recognized  that  the  elements  of  the  lin¬ 
earization  matrix  are  directly  related  to  the  compo¬ 
nents  of  the  computed  measurement  vector,  y(q), 
given  by  Eq.  (12).  Since  this  vector  is  needed  in 
every  cycle  of  Gauss-Newton  iteration  to  compute 
the  modeling  error  vector,  e(4old),  given  by  Eq,  (13), 
it  follows  that  the  above  value  for  Xv  is  “free”  since 
all  terms  are  known  once  e(qold)  has  been  computed. 
Eq.  (36)  provides  an  order  of  magnitude  reduction  in 
the  amount  of  computation  needed  to  obtain  X,  [24, 
25].  This  has  significant  importance  when  applying 
Gauss-Newton  iteration  in  practical  real-time  orien¬ 
tation  tracking  systems. 


Linearization  1.1.1 

Figure  3  is  a  block  diagram  of  the  linearized 
quaternion  attitude  estimation  filter.  The  inputs 
and  n2  are  maneuver  induced  noise  and  rate  sensor 
noise,  respectively.  The  basis  for  linearization  is 
the  assumption  that  in  the  absence  of  measure¬ 
ment  noise  the  computation  of  Aq^n  is  exact  and 
therefore 


Hai  =  <ltrue-4  (38) 


This  assumption  would  be  exact  only  if  y  depended 
linearly  on  q,  which  it  does  not. 

Application  of  Mason’s  formula  to  Figure  3 
produces: 


1 

s 


(39) 


Thus,  with  correct  initial  conditions,  in  the  absence 
of  noise, 


T  =  -J-qtvue  (40) 

regardless  of  the  value  of  k.  This  means  that,  under 
the  linearization  assumptions,  Figure  3  is  a  comple¬ 
mentary  filter  [3]  since,  for  all  k,  if  nx  and  n2  are 
zero,  then  q  =  q^. 


Fig.  3-Block  Diagram  of  Linearized.  System 
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Noise  Response 

Applying  Mason’s  formula  to  noise  disturbances 
nx  and  n2  in  Figure  3,  produces  the  following  low 
pass  filter  transfer  functions. 


k 

s  +  k 


(41) 


Equivalent  results  apply  for  y(t)  and  z(t).  This 
implies  that  any  transient  errors  in  q  resulting  from 
erroneous  initialization  will  persist  for  a  time 
inversely  proportional  to  k.  Specifically 

TAq  =  Y  (49) 

and  for  any  disturbance,  8X,  the  resulting  errors  in 
the  x  component  of  a  will  be 


1 

s  +  k 


(42) 


Eq,  (41)  and  Eq.  (42)  can  be  used  to  find  an  optimal 
k  value  in  Eq.  (18)  based  upon  power  spectral  den¬ 
sity  functions  for  both  the  noise  signals  and  actual 
maneuvering  behavior  of  the  tracked  object. 
Unfortunately,  this  information  will  rarely  be  avail¬ 
able,  so  ad  hoc  “tuning”  of  k  is  more  likely  to  succeed 
in  practical  circumstances  [27], 


e*(t)  -  8*6“^  (60) 

Thus,  it  can  be  predicted  that  any  error  will  be 
reduced  to  37  percent  of  the  initial  value  by  the  time 
t  ~  TAq.  Similar  results  apply  to  8y  and  8Z. 

Since  the  nonlinear  simulation  results  shown  by 
the  lower  line  in  Figure  4  have  the  characteristics  of 
a  typical  first  order  linear  system,  the  value  of  lin¬ 
earization  is  established.  This  theory  provides 
a  framework  under  which  to  choose  filter  gains. 
Broadband  noise  simulation  shows  noise  reduces 
accuracy,  but  the  filter  still  works  well  [4]. 


Response  to  Initial  Condition  Errors 

Eq.  (40)  assumes  that  q  has  been  correctly  ini¬ 
tialized.  In  order  to  understand  how  an  erroneous 
initialization  approaches  qtrue  over  time,  consider 
the  following  static  sensor  scenario.  Suppose  the 
sensor  module  is  mounted  in  a  static  reference 
position  and 

qtrue  =  [1  0  0  0]  (43) 

Assume  that  the  unit  quaternion,  q,  is  incorrect  and 
is  represented  by 

qtrue  =[1  Sx  8y  8J  (44) 

where  all  8  are  small  quantities.  In  the  absence  of 
motion  and  noise,  q  =  0  and  both  nx  and  n3  equal 
zero.  Based  on  Figure  3,  the  initial  value  for  epsilon 

of  e(4o)  is 

s(4>)  =  qtrue  -  4j  =  [0  -Sx  -8y  -8J  (45) 

Since  the  first  component  of  qx  in  Eq.  (45)  will 
always  be  zero,  it  can  be  assumed  that  the  first  com¬ 
ponent  of  Eq.  (43)  will  remained  unchanged  and  q 
will  take  on  the  form 

q  =  (1  x  y  z)  (46) 

The  transfer  function  to  the  scalar  x  from  8X  is 
given  by: 

X(s)_  £j  ___j_  m 

8X  1  +  ks”1  s  +  k 

Employing  the  inverse  Laplace  transform  produces 
the  result 

x(t)  =  8xe“kt  x(t)  =  Sxe"kt  (48) 


Bias  Effects 

Integration  of  a  biased  angular  rate  signal  will 
cause  a  steady  state  error  in  a  complementary  filter. 
To  reduce  this  effect,  following  the  approach 
described  in  [28],  an  initial  estimate  for  bias  can  be 
calculated  by  averaging  rate  sensor  output  prior  to 
maneuvering  and  then  tracking  the  time-varying 
bias  with  a  very  long  time  constant,  low  pass  filter. 


From  Figure  5, 


Time  (aoconda) 


Fig.  4-Simulated  Nonlinear  Filter  Response ,  10  Degree  Offset 
a  =  0.1,  At  =  0.1 
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which  is  the  equation,  for  a  highpass  filter  with 
a  3  db  cutoff  at 

“c  -^-bias  (52) 

Based  on  the  high-pass  nature  of  Eq.  (51),  it  can  be 
seen  that  the  addition  of  bias  estimation  to  the 
quaternion  filter  means  it  will  no  longer  be  comple¬ 
mentary  [3].  This  is  evident  since  constant  rotation 
rates  will  over  time  be  eliminated  from  qtrue.  Thus, 
k  must  be  greater  than  zero  in  Eq.  (18)  in  order  to 
detect  these  rates.  In  [28]  it  is  shown  that  this  effect 
can  be  minimized  by  applying  the  constraint 

k  »  kbifla  (53) 

Note,  however,  that  if  k  is  too  large,  the  filter  may 
become  unstable  or  too  much  maneuver  induced 
error  will  appear  in  q.  Thus,  for  a  given  kbia8,  it  can 
be  expected  there  will  be  an  optimal  k  value. 

Prom  Eq.  (49),  it  can  be  seen  that  k  should  not  be 
too  small  if  the  filter  is  to  converge  in  a  reasonable 
time  period.  On  the  other  hand,  rAq  must  be  larger 
than  the  maneuver  time  constant,  Tmaneuverj  in  order 
to  adequately  suppress  maneuver  noise.  Combining 
this  result  with  Eq.  (53)  leads  to  the  qualitative 
requirement 


Tmanuever  <^'  TAq  ^  Tblqs 

(54) 

l/^manuever  ^  ^  l/^biaa 

(55) 

This  result  in  addition  to  Eq.  (38)  provides  guide¬ 
lines  for  the  selection  of  “reasonable”  values  for 
k  and  At.  With  power  spectral  density  functions  for 
qtrue,  nl3  and  n2,  a  Kalman  filtering  approach  [3] 
could  be  used  for  this  problem.  In  the  absence  of 
such  statistical  information,  gain  values  may  be 
selected  through  experimental  “tweaking”  of  the 
scalar  gain,  k,  in  laboratory  studies. 


A  Simple  Prototype  System  and 
Experimental  Results 

The  quaternion  filter  was  tested  in  a  prototype 
inertial/magnetic  posture  tracking  system.  The 
system  was  designed  with  several  goals  in  mind. 
Among  these  was  validation  of  the  quaternion  filter 
algorithm  in  a  physical  system  and  a  demonstration 
of  the  practicality  and  robustness  of  real-time  iner¬ 
tial  and  magnetic  posture  tracking. 

Figure  6  is  a  diagram  of  the  prototype  system 
hardware.  Depicted  are  three  body-mounted 
inertial/magnetic  sensor  modules  outputting  analog 
signals  to  three  I/O  connection  boards.  The  output 
from  each  connection  board  was  digitized  by  an 
associated  A/D  converter  card.  The  cards  themselves 
are  mounted  in  a  standard  Wintel  desktop  com¬ 
puter.  All  data  processing  and  rendering  calcula¬ 
tions  are  performed  by  software  running  on  this  sin¬ 
gle  processor  machine.  In  order  to  ensure  sufficient 
dynamic  response  to  capture  fast  human  body 
motion,  the  system  update  rate  was  maintained  at 
100  Hz. 

The  prototype  tracking  system  was  capable  of 
simultaneously  estimating  the  attitude  of  three 


Fig.  6-Prototype  Inertial  (Magnetic  Body  Tracking  System 
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rigid-bodies.  To  track  the  posture  of  the  entire 
human  body,  approximately  fifteen  sensor  modules 
would  be. required.  One  sensor  would  be  attached  to 
each  limb  segment  to  be  tracked.  The  exact  number 
of  sensors  needed  would  depend  upon  the  desired 
motion  tracking  detail  to  be  captured. 

The  body  tracking  experiments  discussed  in  the 
following  text  were  conducted  using  a  MARG-0-2 
sensor  fabricated  by  McKinney  Technology  [33].  The 
primary  sensing  components  are  a  Crossbow 
CXL04M3  triaxial  accelerometer  [30],  a  Honeywell 
HMC2003  3-axis  magnetometer  [31]  and  three 
Tokin  CG-16D  series  miniature  angular  rate  sen¬ 
sors  mounted  in  an  orthogonal  configuration  [32]. 
The  MARG-0-2  incorporates  a  capacitive  coupling 
rate  bias  compensation  circuit.  [34]  describes  the 
use  of  a  digital  filter  designed  to  replace  the  capaci¬ 
tive  coupling  circuit.  Sensor  module  calibration  for 
null  values  and  scale  factors  was  accomplished 
without  the  use  of  any  specialized  equipment  using 
a  simple  algorithm.  Possible  non-ortliogonalities 
within  each  sensor  triad  and  misalignments 
between  the  triads  were  ignored  [36]. 

Quantitative  and  qualitative  experiments  to 
obtain  data  related  to  the  accuracy  and  robustness 
of  the  system  under  various  dynamic  and  static 
conditions  have  been  performed.  The  static  experi¬ 
ments  are  related  to  the  stability,  convergence 
properties,  and  accuracy  of  the  orientation  esti¬ 
mates  produced  by  the  quaternion  attitude  filter 
algorithm  when  processing  sensor  data.  The  quali¬ 
tative  experiments  examine  the  performance  of  the 
system  as  a  whole  in  relationship  to  the  goal  of 
robust  posture  estimation.  The  performance  of  the 
system  while  using  differential  weighting  of  sensor 
data,  increased  drift  correction  intervals,  and  the 
effects  of  magnetic  field  variations  have  also  been 
investigated  [4,  37]. 

Static  Stability 

The  drift  characteristics  of  the  quaternion  filter 
algorithm  over  extended  periods  have  been  evalu¬ 
ated  using  static  tests.  Figure  7  graphically  depicts 
the  drift  of  each  of  the  four  components  of  the 
quaternion  estimate  produced  by  the  filter  when 
processing  sensor  data  as  well  as  the  rms  drift  from 
the  initial  orientation  quaternion  estimate.  The 
maximum  possible  difference  between  two  quater¬ 
nions  of  unit  length  is  2.0  or  180  deg.  It  can  thus  be 
observed  through  examination  of  Figure  7  that  aver¬ 
age  total  drift  is  about  0.25  percent.  During  the 
experiment  shown,  the  filter  gain,  k  (Eq.  (18)),  was 
set  to  4.0.  Reducing  the  gain  to  1.0  increases  the 
error  to  1.0  percent.  Further  experiments  [4]  indi¬ 
cated  that  nearly  all  drift  was  due  to  slowly  varying 
bias  errors  in  the  rate  sensors. 


Time  (Seconds) 


Fig,  7 -Quaternion  Real  And  Vector  Parts  and  RMS  Drift  During 
One  Hour  Static  Test  of  Orientation  Estimate  Stability  While 
Processing  MARG-0-2  Sensor  Module  Data ,  k  -  4,0 

Dynamic  Response  and  Accuracy 

Experiments  to  establish  the  accuracy  of  the  ori¬ 
entation  estimates  and  the  dynamic  response  of  the 
system  were  conducted.  These  experiments  were 
completed  by  mounting  a  MAR.G  sensor  on  a  Hass 
rotary  tilt  table.  The  test  procedure  consisted  of 
repeatedly  cycling  a  sensor  through  various  angles 
of  roll,  pitch,  and  yaw  at  rates  ranging  from  10  to 
60  deg/s. 

Figure  8  is  a  typical  result  of  the  dynamic  accuracy 
experiments.  The  estimates  produced  by  the  tracking 
system  are  presented  in  Euler  angle  form.  Accuracy 
is  measured  to  be  better  than  one  degree.  The  over¬ 
all  smoothness  of  the  plot  shows  excellent  dynamic 
response.  The  small  impulses  observed  each  time 
motion  is  initiated  are  due  to  linear  acceleration 
effects  exaggerated  by  the  “whipping”  motion  of  the 
non-magnetic  extension  on  which  the  sensor  module 
was  mounted  during  the  experiments. 


Fig,  8 -Quaternion  Filter  Attitude  Estimates  During  Negative 
90  deg  Roll  Excursions  at  60  Deg  Is,  k  =  1.0 
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The  maximum  value  for  k  can  be  quantitatively 
established  through  a  geometric  series  [4].  The  max¬ 
imum  value  for  which  the  filter  can  be  expected  to 
be  stable  is 


When  working  with  perfect  noiseless  data,  values 
for  k  greater  than  1/At  can  be  expected  to  cause  cor¬ 
rection  “overshoots”  and  oscillations  in  the  attitude 
estimate.  The  effects  of  varying  the  filter  gain  value, 
k,  are  depicted  in  Figure  9.  The  same  exaggeration 
due  to  the  whipping  motion  of  the  non-magnetic 
extension  can  be  seen.  The  top  sub-plot  in  which  the 
filter  gain  was  set  to  2.0  shows  a  smooth  plot  which 


k  =  2 


closely  follows  the  actual  motion  of  the  sensor 
module.  The  middle  sub-plot  shows  that  the  filter  is 
less  able  to  discriminate  against  sensor  noise  with  a 
gain  of  16.0.  It  is,  however,  still  able  to  track  the 
actual  motion  of  the  module.  In  the  bottom  sub-plot 
with  a  gain  of  64.0,  the  filter  estimate  overshoots 
the  rotation  significantly  following  the  end  of 
motion.  The  overshoot  is  followed  by  oscillations  for 
a  period  of  several  seconds. 

In  the  absence  of  statistical  information,  gain 
values  are  selected  through  experimental  “tweak¬ 
ing”  of  the  scalar  gain,  k,  in  laboratory  studies. 
During  posture  tracking  the  filter  gain  is  normally 
set  through  qualitative  observation  of  the  system 
performance.  For  normal  human  motion,  a  filter 
gain  of  lc  =  4.0  was  found  to  produce  stable  and 
accurate  performance  with  no  observable  under¬ 
shoots  of  overshoots. 


k  =  is 


k  =  64 


Summary  and  Conclusions 

This  research  has  demonstrated  an  alternative 
technology  for  tracldng  the  posture  of  an  articu¬ 
lated  rigid  body.  The  technology  is  based  on  the  use 
of  inertial/magnetic  sensors  to  independently 
determine  the  orientation  of  each  link  in  the  rigid 
body.' At  the  core  of  the  system  is  an  efficient 
complementary  filter  that  uses  a  quaternion  repre¬ 
sentation  of  orientation.  The  filter  can  continu¬ 
ously  track  the  orientation  of  human  body  limb 
segments  through  all  attitudes  without  singulari¬ 
ties.  Drift  corrections  are  made  continuously  with 
no  requirement  for  still  periods.  Experimental 
results  indicate  that  the  filter  is  both  accurate  and 
efficient. 

Current  technology  has  permitted  the  construc¬ 
tion  of  sensors  that  are  much  smaller  than  the 
prototype  modules  described  here  [36].  An  optimal 
inertial  sensor  would  have  the  same  size  and  form 
factor  as  a  wristwatch.  It  would  include  an  embed¬ 
ded  microprocessor  on  which,  the  filter  algorithm  is 
implemented  as  well  as  an  analog  to  digital  con¬ 
verter.  The  sensor  would  have  a  self-contained 
power  source  and  would  wirelessly  transmit  orien¬ 
tation  data.  Efforts  are  currently  being  made  to 
untether  the  user  of  the  sensor  system  by  feeding 
sensor  data  to  a  wearable  computer.  Filter  research 
has  continued  with  the  development  of  an  extended 
Kalman  Filter  for  real-time  estimation  of  rigid  body 
orientation  [34,  39].  Quantitative  comparisons  of  the 
dynamic  and  static  performance  of  the  algorithms 
are  currently  underway. 
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Abstract — Orientation  of  a  rigid  body  can  be  determined 
from  measured  gravity  and  local  magnetic  field  vectors.  In  hu¬ 
man  body  tracking  applications,  where  it  is  assumed  linear  ac¬ 
celeration  will  average  to  zero  over  any  extended  period,  triads 
of  accelerometers  and  magnetometers  can  be  used  to  measure 
gravity  and  local  magnetic  field  vectors  in  sensor  coordinates. 
Pitch  and  roll  can  be  determined  using  only  acceleromter  data. 
Due  to  deviations  of  the  direction  of  magnetic  field  vector 
between  locations,  it  is  desirable  to  use  magnetic  data  only 
in  calculations  related  to  the  azimuth.  The  TRIAD  algorithm 
can  be  used  to  algebraically  solve  this  problem.  Alternatively, 
some  formulation  of  the  QUEST  algorithm  can  be  used  to  find 
an  optimal  solution  based  on  a  given  set  of  measurements. 
This  paper  presents  an  intuitive  geometric  3-DOF  orientation 
estimation  algorithm  with  physical  meaing,  called  the  factored 
quaternion  algorithm.  Through  a  derivation  based  on  half-angle 
formulas,  the  algorithm  sequentially  calculates  three  angles  and 
produces  a  quaternion  output  to  represent  orientation.  The 
use  of  magnetic  data  is  restricted  to  determination  of  rota¬ 
tion  about  the  vertical.  Thus,  magnetic  variations  cause  only 
azimuth  errors.  A  singularity  avoidance  method  is  introduced 
that  allows  the  algorithm  to  track  through  all  orientations. 
Experimental  results  demonstrate  that  the  proposed  algorithm 
has  an  overall  accuracy  that  is  essentially  identical  to  that  of 
the  TRIAD  and  QUEST  algorithms,  and  has  a  computational 
efficiency  that  is  comparable  to  the  TRIAD  and  better  than 
the  QUEST. 

Index  Terms — Motion  measurement;  inertial  sensors;  mag¬ 
netic  sensors;  accelerometers,  orientation  estimation;  QUEST 
algorithm;  quaternions;  factored  quaternion  algorithm. 

I.  Introduction 

C CURATE  real-time  tracking  of  the  orientation  or 
attitude  of  rigid  bodies  has  applications  in  robotics, 
aerospace,  underwater  vehicles,  synthetic  reality,  and  oth¬ 
ers.  For  synthetic  reality  applications,  the  human  body 
can  be  viewed  as  an  articulated  rigid-body  consisting  of 
approximately  fifteen  links.  If  the  orientation  relative  to 
a  fixed  reference  frame  can  be  determined  for  each  of  the 
links,  then  the  overall  posture  of  the  human  subject  can 
be  accurately  rendered  and  communicated  in  real-time. 
The  orientation  of  an  individual  limb  segment  can  be 
measured  through  the  attachment  of  an  inertial/magnetic 
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sensor  module.  Such  sensor  modules  typically  contain 
a  triad  of  orthogonally  mounted  accelerometers  and  a 
triad  of  orthogonally  mounted  magnetometers.  Under  the 
assumption  that  human  limb  acceleration  is  bounded  and 
averages  to  zero  over  any  extended  period  of  time,  the 
accelerometers  are  used  to  measure  the  gravity  vector 
relative  to  the  coordinate  frame  of  the  sensor  module. 
The  magnetometers  serve  a  similar  function  for  the  local 
magnetic  field  vector.  In  dynamic  applications,  a  triad  of 
angular  rate  sensors  can  be  added  as  a  high  frequency 
source  of  orientation  information.  The  availability,  low- 
cost,  and  small-size  Micro-Electro-Mechanical  Systems 
(MEMS)  sensors  has  made  it  possible  to  build  wrist  watch 
sized,  self-contained  inertial/magnetic  sensor  modules  [4], 
[15].  These  modules  can  be  used  to  accurately  track 
orientation  in  real-time.  This  technique  of  orientation 
estimation  is  dependent  only  on  passive  measurement  of 
physical  quantities  that  are  directly  related  to  the  rate  of 
rotation  and  orientation  of  a  rigid  body.  Since  no  generated 
signals  are  involved,  there  are  no  restrictions  on  the  range 
of  operation.  All  latency  in  such  a  system  is  due  to  the 
computational  demands  of  the  data  processing  algorithms. 

In  body  tracking  applications  based  on  the  use  of 
small  inertial/magnetic  sensors  [4],  the  gravity  and  local 
magnetic  field  vectors  are  often  measured  and  compared 
to  reference  vectors  in  order  to  determine  orientation.  In 
the  case  of  the  gravity  vector,  the  assumption  that  it 
is  fixed  leads  to  no  difficulties  since  this  vector  points 
straight  down  in  any  inertial  frame  located  on  or  near 
the  surface  of  the  earth.  Making  the  same  assumption 
regarding  the  local  magnetic  field  vector  can  lead  to 
problems.  In  a  typical  room  setting  the  direction  of  the 
local  magnetic  field  vector  can  be  expected  to  vary  due  to 
the  presence  of  ferrous  objects  or  electrical  appliances.  In 
inertial/magnetic  tracking  algorithms,  the  local  magnetic 
field  vector  is  commonly  treated  as  a  fixed  reference.  It 
is  assumed  that  this  reference  will  remain  constant.  If  it 
does  not,  algorithms  may  be  prone  to  errors  not  only  in 
azimuth,  but  also  in  pitch  and  roll  as  well. 

The  TRIAD  algorithm  [11]  is  a  single  frame  determin¬ 
istic  method  for  calculating  the  attitude  of  a  rigid  body 
relative  to  a  Earth  fixed  reference  frame.  The  algorithm 
requires  normalized  measurements  of  two  nonparallel  ref¬ 
erence  vectors  as  input.  Since  the  problem  is  overspecified, 
the  TRIAD  algorithm  works  by  throwing  away  some 
components  of  these  measurement  vectors.  It  is  used  to 


REGULAR  PAPER  SUBMISSION  TO  JOURNAL  OF  ROBOTIC  SYSTEMS  2006 


2 


agebraically  solve  for  the  3x3  orthogonal  orientation 
matrix  A,  such  that 

bv  =  AEv  (1) 

where  Ev  and  bv  are  representations  of  a  vector  v  in 
Earth  and  body  coordinates  respectively.  The  algorithm 
constructs  two  triads  of  orthonomal  unit  vectors.  The  two 
triads  are  the  components  of  an  inertial  frame  expressed  in 
both  the  body  and  Earth  fixed  reference  frames.  Let  ba  and 
bm  be  accelerometer  and  magnetometer  measurements 
relative  to  the  body  frame  of  the  the  gravity  and  magnetic 
field  reference  vectors,  Eg  and  Em.  The  reference  vectors 
are  expressed  relative  to  an  Earth  fixed  frame.  The  first 
triad  is  given  by 


h  =E  g 

(2) 

( Eg  xE  mn) 

32  \Eg  xE  m\ 

(3) 

s  _  (Eg  x  ( Eg  xE  to)) 

\Eg  xE  m\ 

(4) 

The  second  triad  is  given  by 

fi  =b  a 

(5) 

(ba  xb  m) 
r2  =  lb  | 

(6) 

\°m  x°  m\ 

( ba  x  (ba  xb  m)) 

3  \baxb  ml 

(7) 

The  triads  are  then  used  to  create  measurement  and 
reference  matrices  such  that 


M-mea  —  [?T  ^ 2  U}]  Tfrej  —  [<§1  <§2  ^3]  (8) 

The  orientation  matrix,  A ,  representing  the  attitude  of  a 
rigid  body  is  then  simply 

A  =  MmeaM?ef  (9) 

It  should  be  noted  that  if  the  measurements  of  the  gravity 
and  the  magnetic  field  are  ordered  as  described  above  the 
cross-products  used  to  caculate  82  and  r 2  eliminate  any 
contribution  of  the  magnetic  measurements  relative  to  the 
vertical.  Thus,  pitch  and  roll  components  of  orientation 
are  determined  using  only  acceleromter  measurements. 

The  QUEST  (QUaternion  ESTimator)  algorithm  is 
an  optimal  algorithm  for  single-frame  estimation  of  a 
quaternion  that  represents  the  attitude  of  a  rigid  body 
relative  to  a  fixed  coordinate  system.  The  algorithm  was 
created  to  solve  Wahba’s  problem  [10]  in  the  context 
of  spacecraft  attitude  determination.  Given  a  set  of  3- 
dimensional  known  reference  unit  vectors  Vi ,  V2,  . . . ,  Vn , 
and  a  set  of  the  corresponding  observation  or  measurement 
unit  vectors  W\ ,  W2 ,  . . . ,  Wn  (which  could  be  the  direction 
of  the  sun  or  a  star  observed  from  a  spacecraft  measured 
in  the  spacecraft’s  body  frame),  Wahba’s  problem  is  to 
find  the  least  squares  estimate  of  spacecraft  attitude  by 
minimizing  the  following  loss  function 

L(A)  =  -  AVi)T(Wi  -  AVi )  (10) 


with  respect  to  the  3x3  orthogonal  orientation  ma¬ 
trix  A,  where  a i,  <22,. . .,  an  are  nonnegative  weighting 
coefficients.  The  minimum  number  of  measurement  and 
reference  vector  pairs  is  two.  Early  solutions  to  Wahba’s 
problem  directly  computed  the  orientation  matrix  A  [12]. 
Davenport  [13]  introduced  a  method  of  parameterizing  the 
orietation  matrix  by  a  unit  quaternion  q ,  and  proved  that 
the  loss  function  (10)  can  be  transformed  into  a  quadratic 
gain  function  of  the  unit  quaternion  in  the  form  of 

n 

G{A(q))=^2,ai-L(A(q))=qTKq  (11) 

i=  1 

where  K  is  a  4  x  4  matrix  constructed  from  the  refer¬ 
ence  vectors  V$,  measurement  vectors  Wi,  and  weighting 
coefficients  cq,  i  =  1,  2,  . . .,  n.  Based  on  Davenport’s 
work,  Shuster  and  Oh  derived  the  QUEST  algorithm 
[11],  and  showed  that  the  optimal  quaternion  q  that 
maximizes  the  gain  function  (11)  while  satisfying  the  unit 
quaternion  (unit  norm)  constraint  is  the  eigenvector  of  the 
K  matrix  corresponding  to  the  largest  eigenvalue  of  K. 
Thus,  the  problem  is  reduced  to  finding  the  eigenvalues 
and  eigenvectors  of  a  4  x  4  matrix. 

Extensive  research  has  been  conducted  to  investigate 
the  use  of  inertial/magnetic  sensor  modules  for  posture 
estimation  in  human  body  tracking  applications.  Foxlin 
et  al.  describes  two  commercial  systems  based  on  the 
use  of  sensor  modules  containing  accelerometers,  magne¬ 
tometers,  and  angular  rate  sensors  [1],  [2].  The  described 
algorithm  is  designed  for  head  tracking  applications  and 
requires  still  periods  to  correct  for  inertial  drift.  Bachman 
et  al.  proposed  a  quaternion-based  complementary  filter 
for  human  body  motion  tracking  [3],  [4].  The  filter  is  able 
to  track  through  all  orientations  without  singularities  and 
continuously  correct  for  drift  without  a  need  for  stationary 
periods  using  data  from  inertial/magnetic  sensor  module 
containing  nine  sensors.  Gallagher  et  al.  presents  a  simpler 
complementary  filter  algorithm  with  lower  computational 
complexity  in  [5].  Luinge  describes  a  Kalman  filter  de¬ 
signed  for  human  body  tracking  applications.  The  filter 
is  based  on  the  use  of  only  accelerometers  and  rate 
sensors.  Drift  about  the  vertical  axis  is  reduced  by  limiting 
body  segment  orientation  using  a  kinematic  human  body 
model  [6] .  Rather  than  estimating  individual  limb  segment 
orientations  relative  to  an  Earth  fixed  reference  frame, 
Zhu  and  Zhou  determine  joint  angles  in  axis/angle  form 
using  the  data  from  the  two  nine-axis  sensors  mounted  on 
the  inboard  and  outboard  sides  of  the  joint  [7].  Yan  and 
Yuan  describe  an  orientation  tracking  algorithm  that  uses 
low  cost  sensor  modules  to  take  two  axis  measurements 
of  gravity  and  the  local  magnetic  field  [8].  In  a  manner 
similar  to  the  method  described  in  this  paper,  elevation, 
roll  and  azimuth  angles  are  sequentially  calculated.  The 
angles  are  used  to  construct  rotation  matrices  and  the 
use  of  trigonometric  functions  is  required.  The  method  is 
limited  to  orientation  tracking  within  a  hemisphere.  In  [9], 
Gebre-Egziabher  et  al.  describe  an  attitude  determination 
algorithm  for  aircraft  applications.  The  algorithm  is  based 
on  a  quaternion  formulation  of  Wahba’s  problem  [10], 
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where  magnetometer  and  accelerometer  measurements  are 
used  to  determine  attitude. 

This  paper  presents  an  alternative  algorithm  for  es¬ 
timating  orientation  based  on  a  set  of  measurements 
from  triads  of  orthogonally  mounted  magnetometers  and 
accelerometers.  It  is  called  the  factored  quaternion  algo¬ 
rithm  (FQA).  It  is  a  intuitive  alternative  to  the  TRIAD 
and  QUEST  slgorithms.  Local  magnetic  field  data  is 
used  only  in  azimuth  angle  calculations.  This  decoupling 
of  accelerometer  and  magnetometer  data  eliminates  the 
influence  of  magnetic  variations  on  calculations  that  de¬ 
termine  pitch  and  roll.  Through  a  derivation  based  on 
half-angle  formulas,  the  computational  cost  of  computing 
trigonometric  functions  is  avoided.  The  algorithm  pro¬ 
duces  a  quaternion  output.  It  is  able  to  track  through 
all  orientations  without  singularities.  Experimental  results 
in  which  the  factored  quaternion  algorithm  is  compared 
with  the  TRIAD  and  QUEST  algorithms  indicate  that  it 
has  nearly  identical  accuracy  at  a  comparable  or  lower 
computational  expense. 

The  primary  contributions  of  this  work  are: 

•  Derivation  of  a  new  geometrically  intuitive  algorithm 
for  determining  orientation  relative  to  an  Earth  fixed 
reference  frame  based  on  a  set  of  accelerometer  and 
magnetometer  measurements. 

•  A  singularity  avoidance  method  that  allows  the  algo¬ 
rithm  to  track  through  all  orientations. 

•  Experimental  results  which  validate  the  performance 
of  the  algorithm  and  compare  it  to  more  established 
methods. 

The  rest  of  this  paper  is  organized  as  follows.  Section 
II  presents  a  derivation  of  the  factored  quaternion  al¬ 
gorithm.  Section  III  describes  experiments  in  which  the 
factored  algorithm  is  compared  to  the  QUEST  algorithm 
for  efficiency  and  accuracy.  The  ability  of  the  algorithm 
to  track  through  all  orientations  without  singularities  is 
demonstrated  as  is  it’s  decoupling  property.  The  final 
section  discusses  the  experimental  results  and  provides 
a  summary. 

II.  Factored  Quaternion  Algorithm 

The  factored  quaternion  algorithm  presented  in  this 
section  is  for  estimating  the  orientation  of  a  rigid  body 
based  on  Earth  gravity  and  magnetic  field  measurements 
[14].  Sensor  modules  such  as  the  MARG  III  described 
in  [15]  contain  a  triad  of  accelerometers  and  a  triad  of 
magnetometers,  and  can  be  used  to  provide  measurement 
data  for  the  factored  quaternion  algorithm. 

In  a  typical  application,  a  sensor  module  is  employed  as 
a  strap  down  inertial  measurement  unit  (IMU)  attached 
to  a  rigid  body  whose  orientation  is  to  be  determined. 
To  facilitate  the  analysis,  it  is  convenient  to  define  three 
coordinate  systems.  An  Earth- fixed  coordinate  system 
xeyeze  is  defined  to  follow  the  North-East-Down  (NED) 
convention,  that  is,  xe  points  to  north,  ye  points  to  east, 
and  ze  points  down.  A  body  coordinate  system  ^bjb^b 
is  attached  to  the  rigid  body  whose  orientation  is  to 


be  measured.  The  sensor  module  has  its  own  coordinate 
system  xsyszs  corresponding  to  the  axes  of  three  orthog¬ 
onally  mounted  accelerometers /magnetometers.  Since  the 
sensor  module  is  rigidly  attached  to  the  rigid  body,  the 
body  coordinate  system  x^y^z^  differs  from  the  sensor 
coordinate  system  xsyszs  by  a  constant  offset.  For  the 
convenience  of  discussions,  in  what  follows,  the  body 
coordinate  system  is  assumed  to  coincide  with  the  sensor 
coordinate  system. 


A.  Quaternion  Rotation  Operator 

Unit  quaternions  can  be  used  to  perform  rotation  oper¬ 
ations  in  the  3-D  space  [16].  In  this  paper,  the  following 
notation  will  be  used  to  represent  a  quaternion  q : 


q  =  (<7o  q\  <?2  53)  (12) 

where  qo  is  the  scalar  (or  real)  part  and  [q-\  q->  qz]T  is 

the  vector  part.  Let 


u  = 


ui 

u2 

U3 


(13) 


be  a  unit  vector  in  the  3-D  space.  The  following  unit 
quaternion 

g  =  cos^(l  0  0  0)  +  sin^(0  ui  U2  U3)  (14) 

is  commonly  utilized  to  perform  rotation  operations. 
Specifically,  for  any  vector  v  =  [v±  V2  vs]T  in  the  3- 
dimensional  space,  the  following  operation 


v'  =  q  vq 


(15) 


produces  a  vector  v'  by  rotating  the  vector  v  about  the  axis 
defined  by  u  by  an  angle  9.  In  the  above,  all  multiplications 
are  quaternion  multiplications,  and  v  and  v'  are  treated 
as  a  pure  quaternion  whose  real  part  is  zero,  g_1  is  the 
inverse  quaternion  of  q  [16]. 


B.  Elevation  Quaternion 

A  rigid  body  is  said  to  be  in  its  reference  orientation 
when  its  x^y^z^-axes  are  aligned  with  those  of  the  Earth 
coordinate  system.  It  is  known  that  a  rigid  body  can  be 
placed  in  an  arbitrary  orientation  by  first  rotating  it  about 
its  z-axis  by  an  angle  ijj  (azimuth  or  yaw  rotation),  then 
about  its  y-axis  by  angle  0  (elevation  or  pitch  rotation), 
and  finally  about  its  x-axis  by  angle  (bank  or  roll 
rotation). 

In  order  to  derive  a  quaternion  describing  only  elevation, 
it  is  useful  to  note  that  when  a  rigid  body  is  moving  at 
a  constant  velocity  and  is  in  a  fixed  orientation,  then  an 
accelerometer  measures  only  gravity.  Furthermore,  the  x- 
axis  accelerometer  senses  only  the  component  of  gravity 
along  the  x-axis,  and  this  component  in  turn  depends  only 
on  elevation  angle.  This  can  be  seen  from  the  following 
argument.  Starting  with  the  rigid  body  in  its  reference 
orientation,  the  x-axis  accelerometer  is  perpendicular  to 
gravity  and  thus  registers  zero  acceleration.  The  y-axis 
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accelerometer  also  reads  zero  while  the  z-accelerometer 
reads  -g.  If  it  is  then  rotated  in  azimuth  about  its  z-axis, 
the  x-axis  accelerometer  still  reads  zero,  regardless  of  the 
azimuth  angle.  If  the  rigid  body  is  next  pitched  up  through 
an  angle  # ,  the  x-axis  accelerometer  will  read: 

ax  =  gsin#  (16) 

and  the  z-axis  accelerometer  will  read: 

az  =  —geos  6  (17) 

where  g  =  9.81ra/s2  is  the  gravitational  acceleration,  and 

a  =  ay  (18) 

_  az 

is  the  acceleration  vector  in  the  body  coordinate  system. 
For  convenience,  the  accelerometer  and  magnetometer 
output  from  a  sensor  module  is  normalized  to  a  unit  vec¬ 
tor.  Let  a  denote  the  normalized  vector  of  the  acceleration 
measurements: 

ax 

ay  (19) 

az 

where  \a\  is  the  norm  of  the  acceleration  vector  a.  It  follows 
from  equation  (16)  that  the  value  for  sin  #  can  be  expressed 
as: 

sm6  =  ax.  (20) 


The  value  for  cos  #  can  be  computed  from 


It  should  be  noted  that  a  positive  value  for  cos  #  is  assumed 
in  the  above  equation.  This  is  because  the  elevation  angle 
#  is  by  convention  restricted  to  the  range  of  — 7r/2  <  0  < 
7r/2.  It  is  noted  that  if  the  rigid  body  is  rolled  about  its 
x-axis,  equation  (17)  will  change,  but  equation  (16)  will 
remain  the  same.  This  means  that  equation  (16)  holds  for 
any  orientation  of  the  rigid  body. 

In  order  to  obtain  an  elevation  quaternion  using  equa¬ 
tion  (14),  a  value  is  needed  for  sin(#/2)  and  cos(#/2). 
From  trigonometric  half- angle  formulas,  half- angle  values 
are  given  by 


sin-  =  sign(sin  6)y/(  1  —  cos  #)/ 2  (22) 


COS  -  =  y/(l  +  COS#)/2 


where  sign()  is  the  sign  function  that  returns  +1  for 
positive  arguments  and  -1  for  negative  arguments.  The 
sign  function  is  not  needed  in  equation  (23)  since  cos(#/2) 
is  always  positive  within  the  elevation  angle  range. 

Elevation  is  a  rotation  about  y-axis.  The  unit  quaternion 
representing  elevation  motion  can  now  be  computed  using 
equation  (14)  and  values  for  the  half  angle  trigonometric 
functions  as  follows: 


C.  Roll  Quaternion 

The  acceleration  measured  by  the  z-axis  accelerometer 
with  roll  angle  =  0  is  given  by  equation  (17).  Changing 
azimuth  does  not  alter  this  measurement,  but  changing 
roll  does.  A  more  general  formula  for  y-axis  accelerometer 
reading  is: 

ay  =  —  g  cos  #  sin  0.  (25) 

Likewise,  the  z-axis  accelerometer  will  read: 

az  =  —  g  cos  #  cos  </>.  (26) 

In  terms  of  the  normalized  acceleration  measurement,  the 
two  equations  above  can  be  written  as: 

ay  =  —  cos#  sin  0  (27) 

az  =  —  cos#  cos  0  (28) 

where  the  value  for  cos#  was  determined  in  equation  (21). 
If  cos#  is  not  equal  to  zero,  the  values  of  sin 0  and  cos </> 
can  be  easily  determined  by: 

sine/)  =  —  dy/cos#  (29) 

cos  0  =  — d^/cos#.  (30) 

If  cos#  is  equal  to  zero,  it  means  that  x-axis  of  the  body 
coordinates  is  vertically  oriented.  In  such  cases,  the  roll 
angle  is  undefined  and  it  can  be  assumed  to  have  a  value 
equal  to  zero.  The  range  of  the  roll  angle  <j)  is  by  convention 
restricted  to  —tt  <  (f)  <  tt.  The  half  angle  values  for  <f>  can 
be  computed  in  a  manner  similar  to  equations  (22)  and 
(23)  with  one  exception.  When  cos  0  =  —1  and  sin  0  =  0, 
the  use  of  equations  (22)  and  (23)  will  result  in  a  value 
of  zero  for  both  sin(0/2)  and  cos(0/2).  This  case  can  be 
treated  in  implementation  by  assigning  a  value  of  one 
to  the  sign  function  when  its  argument  is  zero.  Having 
obtained  the  half  angle  values  for  the  roll  angle  0,  the  roll 
quaternion  is  computed  as  follows: 

qr  =  cos^(l  0  0  0)  +  siiA(0  1  0  0).  (31) 

D.  Azimuth  Quaternion 

Since  azimuth  rotation  has  no  effect  on  the  estimation 
of  roll  or  elevation  quaternions  from  accelerometer  data, 
the  strategy  to  be  employed  in  this  paper  for  azimuth 
quaternion  estimation  is  to  first  solve  for  the  elevation 
and  roll  quaternions.  These  can  then  be  used  to  rotate 
the  normalized  magnetic  field  measurement  vector  in  the 
body  coordinate  system 

bmx 

bmy  (32) 

bmz 

into  the  Earth  coordinate  system  by  the  quaternion 
rotation  operation: 

em  =  qe  qr  hm  q~x  q~x .  (33) 

In  the  above,  hm  stands  for  the  pure  quaternion  of  the 
3-dimensional  vector  itself,  i.e.,  bm  =  (0  bmx  bmy  bmz). 
The  same  convention  is  used  for  em.  In  the  absence  of 
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measurement  error,  em  should  agree  with  the  known  local 
normalized  magnetic  field  vector  n  =  [nx  ny  nz]T , 
except  for  the  effects  of  azimuth  rotation  on  the  sensor 
magnetometer  readings.  In  such  a  case,  nz  =  and 


cos  ip 

—  sin  ip 

emx 

sin.'ip 

cos  ip 

_  emy  _ 

(34) 


where  ip  is  the  azimuth  angle.  Before  proceeding  further, 
it  should  be  noted  that  equation  (34)  implies  that  the 
two  2-dimensional  vectors  differ  only  in  orientation.  In 
fact,  experimental  data  show  that,  in  the  presence  of 
measurement  noise,  they  may  also  differ  in  length.  To 
compensate  for  this  effect,  the  vectors  on  both  sides  of 
equation  (34)  can  be  normalized.  Specifically,  let  the 
normalized  local  magnetic  field  reference  vector  in  the 
horizontal  plane  be: 


N  = 


Nx 

Ny 


TTx 

Tly 


(35) 


and  the  corresponding  quantity  measured  by  the  magne¬ 
tometer  be: 


M  = 


Mx 

My 


emx 

emy 


(36) 


With  these  definitions,  equation  (34)  becomes: 


Nx  ' 

cos  ip 

—  sin  ip 

'  Mx  ‘ 

Ny  _ 

sin  Tp 

cos  ip 

_  My  _ 

from  which  the  value  of  cos^  and  smip  can  be  solved  as: 


COS  ip 

Mx 

My  - 

‘  Nx  ' 

sin  ip 

.  ~My 

Mx  _ 

.  Nv  . 

The  azimuth  angle  'ip  is  restricted  to  the  range  —it  <  'ip  < 
it.  The  half  angle  formulas  given  by  equations  (22)  and 
(23)  can  again  be  used  to  compute  the  half  angle  values 
of  rip.  The  azimuth  quaternion  is  then  given  by: 


c[a  —  cos  "2" (1  0  0  0)  +  sin^(0  0  0  1).  (39) 


Having  obtained  all  three  rotation  quaternions,  the 
quaternion  estimate  representing  the  orientation  of  the 
rigid  body  is  finally  given  by: 


Q  —  Qa  Qe  Qr- 


(40) 


E.  Singularity  Avoidance  in  Implementation 

The  factored  quaternion  algorithm  presented  above 
takes  the  normalized  acceleration  measurement  vector  and 
local  magnetic  field  measurement  vector  as  its  input,  and 
produces  a  quaternion  as  its  output.  It  is  a  single- frame 
algorithm,  that  is,  it  takes  measurements  at  a  single 
instant  of  time,  and  produces  an  output.  It  does  not 
require  a  history  of  measurements  at  multiple  instants 
of  time. 

From  the  two  measurement  vectors,  the  half  angle 
value  of  each  rotation  angle  is  first  computed.  Then  the 
corresponding  quaternion  for  each  rotation  is  computed. 
Finally,  the  overall  orientation  quaternion  is  computed  by 
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Fig.  1.  Roll,  pitch,  yaw  angles,  singularity  condition,  and  switch 
flag  during  a  180°  rotation  in  pitch  axis  with  ideal  simulated  data. 


equation  (40).  It  should  be  emphasized  that  the  algorithm 
does  not  evaluate  trigonometic  functions  at  any  step. 

Although  quaternions  when  used  to  represent  the  3- 
dimensional  orientation  do  not  have  singularities,  the 
factored  quaternion  algorithm  described  above  uses  three 
angles  to  derive  the  quaternion  estimate.  It  is  known  that 
any  three-parameter  representation  of  the  3-D  orientation 
is  inevitably  singular  at  some  point  [17].  Without  excep¬ 
tion,  the  factored  quaternion  algorithm  has  a  singularity, 
so  does  the  QUEST  algorithm.  The  QUEST  algorithm 
uses  the  Gibbs  vector 


P  = 


1 

qo 


qi 

Q2 

qs 


(41) 


in  its  derivation  and  is  at  a  singular  point  if  qo  =  0. 
A  method  similar  to  the  method  of  sequential  rotations 
discussed  in  [18]  is  described  below  to  avoid  singularities 
in  the  numerical  implementation.  A  singularity  occurs  in 
the  factored  quaternion  algorithm  if  the  elevation  angle 
is  ±90°.  This  happens  when  cos#  =  0  or  equivalently 
az  =  0  in  equations  (29)  and  (30).  In  implementation, 
the  first  step  of  the  algorithm  is  to  check  the  value  of  az. 
If  the  absolute  value  of  az  is  smaller  than  a  predefined 
constant  e  (e.g.,  e  =  0.1),  the  procedures  described  below 
are  implemented  to  circumvent  the  numeric  difficulty  of 
having  a  small  number  in  the  denominator. 

If  az  <  e,  the  elevation  angle  is  close  to  ±90°.  The  nor¬ 
malized  acceleration  measurement  vector  a  and  magnetic 
field  measurement  vector  hm  in  the  body  frame  will  be 
rotated  about  the  body  coordinate  ^-axis  by  an  angle 
a  to  obtain  the  following  offset  (rotated)  measurement 
vectors: 


^offset  qa  d  (Zq; 

bmoSSset  =  qa  bm  q~x 


(42) 

(43) 
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where  Offset  is  the  offset  (rotation)  quaternion  given  by 

ga  =  cos  —  (1  0  0  0)  +  sin  —  (0  0  1  0).  (44) 

Under  the  condition  of  az  <  e,  the  offset  measurement 
vectors  will  be  used  in  place  of  the  original  measurement 
vectors  to  carry  out  the  factored  quaternion  algorithm. 
The  resultant  orientation  quaternion  estimate  from  equa¬ 
tion  (40)  in  this  case  is  called  galt. 

The  value  of  a  can  be  chosen  arbitrarily  as  long  as  it  is 
sufficiently  away  from  zero.  It  is  chosen  to  be  20°  in  this 
discussion.  Rotating  measurement  vectors  about  yb- axis  by 
20°  is  equivalent  to  rotating  the  (original)  body  coordinate 
system  xbybzb  to  a  temporarily  offset  body  coordinate 
system  x.'by'bzb  about  yb- axis  by  -20°.  galt  represents  the 
orientation  of  x'hybz'h  in  the  Earth  coordinate  system.  The 
quaternion  estimate  q  representing  the  orientation  of  the 
original  body  coordinate  system  -x.bybzb  is  given  by  the 
following  compound  quaternion  (i.e.,  rotating  y^by'bzlh  back 
to  x byb^b  about  y^-axis  by  20° ): 

q  =  Lit  qa ■  (45) 

To  demonstrate  how  the  singularity  avoidance  method 
described  above  works,  ideal  measurements  as  well  as 
noisy  measurements  for  a  180°  rotation  in  pitch  axis  were 
synthetically  generated.  Figure  1  shows  the  results  with 
ideal  measurements.  The  top  three  plots  are  trajectories 
of  roll,  pitch,  and  yaw  angles.  The  bottom  two  plots  depict 
the  value  of  cos  #  and  the  switch  flag.  The  value  of  cos  #  is 
an  indication  of  the  singularity  condition,  and  the  switch 
flag  indicates  when  the  singularity  avoidance  method  is 
invoked.  As  expected,  the  pitch  angle  increases  from  0  to 
90°  while  the  roll  and  yaw  angles  remain  at  zero  during 
the  first  half  period.  As  the  pitch  angle  approaches  90°, 
the  value  of  cos#  drops  nearly  to  zero.  When  cos #  is 
less  than  e  (whose  value  is  chosen  as  0.1  in  this  testing), 
the  singularity  avoidance  method  is  activated  during  the 
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Fig.  2.  Roll,  pitch,  yaw  angles,  singularity  condition,  and  switch  flag 
during  a  180°  rotation  in  pitch  axis  with  noisy  simulated  data.  The 
parameters  used  are:  e  =  0.1  and  the  offset  angle  a  =  20  degrees. 


Components  of  estimated  quaternion  from  factored  quaternion  algorithm 
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Fig.  3.  Components  of  the  estimated  quaternion  during  a  180° 
rotation  in  pitch  axis  with  noisy  simulated  data. 


period  of  sample  numbers  from  about  820  to  980  as  seen 
in  Figure  1.  During  this  period,  the  value  of  cos#  is  lifted 
upwards  to  be  away  from  zero.  The  value  of  the  offset 
angle  a  is  chosen  to  be  20°. 

Owing  to  the  conventional  choice,  the  pitch  angle  is 
limited  from  -90°  to  90°.  As  a  result,  the  orietation  of 
a  95°  pitch,  0°  roll,  and  0°  yaw  is  depicted  as  85°  pitch, 
180°  roll,  and  180°  yaw  in  Figure  1.  This  is  the  reason  why 
the  pitch  angle  increases  from  0  to  90°  and  then  decreases 
from  90°  to  0  while  the  actual  rotation  increases  from  0 
to  180°. 

Figure  2  shows  the  results  with  noisy  measurements 
for  the  same  rotational  motion  as  in  Figure  1.  Noises 
were  introduced  using  a  random  number  generator.  It 
is  noted  that  the  switch  flag  flipped  many  times,  and 
the  value  of  cos#  was  kept  above  e  =  0.1  at  all  times. 
The  trajectory  of  the  pitch  angle  follows  the  same  rise 
and  fall  pattern  as  in  Figure  1  except  with  added  noise. 
The  roll  and  yaw  angles  flipped  from  0  to  180°  numerous 
times,  signifying  that  the  pitch  angle  jumped  above  and 
below  90°.  Figure  1  and  Figure  2  plot  the  trajectory  of 
the  roll,  pitch,  and  yaw  angles  for  visualization  purposes. 
Although  there  are  jumps  in  roll  and  yaw,  there  are  no 
jumps  in  the  trajectory  of  the  estimated  quaternion  as  seen 
from  the  corresponding  plot  of  the  estimated  quaternion 
components  shown  in  Figure  3. 

III.  Experimental  Results 

The  experimental  results  described  in  this  section  com¬ 
pare  the  factored  quaternion  and  QUEST  algorithms. 
They  also  exhibit  the  unique  properties  of  the  factored 
quaternion  algorithm.  The  first  set  of  experiments  con¬ 
stitutes  a  side-by-side  comparison  of  the  two  algorithms 
for  static  and  dynamic  accuracy.  In  these  experiments, 
an  inertial/magnetic  sensor  module  was  subjected  to 
a  series  of  known  rotations  at  several  different  rates. 
Both  the  factored  quaternion  and  QUEST  algorithms 
were  tested  first  using  raw  accelerometer  data  and  then 
low-pass  filtered  data.  The  second  set  of  experiments 
demonstrate  the  effectiveness  of  the  singularity  avoid 
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method  presented  in  the  previous  section.  The  third 
set  of  experimental  results  demonstrates  the  decoupling 
property  of  the  factored  quaternion  algorithm.  In  these 
experiments  a  stationary  inertial/magnetic  sensor  module 
was  subjected  to  a  magnetic  field  exhibiting  a  varying 
direction  and  magnitude  by  moving  a  ferrous  object  in  the 
vicinity  of  the  sensor  module.  Finally  a  rough  comparison 
of  the  computational  efficiency  of  the  QUEST  and  factored 
quaternion  algorithms  is  made. 

Sensor  data  for  the  experiments  was  collected  using  a 
MARG  III  inertial/magnetic  sensor  module  which  was 
designed  by  the  authors  and  fabricated  by  McKinney 
Technology  [15].  Primary  sensing  components  for  this  unit 
include  a  pair  of  two  axis  Analog  Devices  ADXL202E  mi- 
cromachined  accelerometers,  and  Honeywell  HMC1051Z 
and  HMC1052  one  and  two-axis  magnetometers.  Overall, 
dimensions  of  the  MARG  III  unit  are  approximately  0.7”  x 
1.2”  x  1.0”.  Though  the  MARG  III  units  contain  angular 
rate  sensors,  no  rate  data  was  used  in  the  experiments 
described  in  this  paper. 


A.  Testing  of  Static  and  Dynamic  Accuracy 


Acceleration 

Estimated 

Measurement 

Low-Pass 

QUEST/Factored 

quaternion 

a 

Filter 

Quaternion  Algorithm 

q 

Fig.  5.  Block  diagram  of  the  QUEST/factored  quaternion  algo¬ 
rithms  with  a  low-pass  filter. 
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Controlled  rotations  of  the  sensor  modules  were  per¬ 
formed  by  placing  an  inertial/magnetic  sensor  module 
on  a  HAAS  precision  tilt  table.  The  table  has  two 
degrees  of  freedom  and  is  capable  of  positioning  with 
an  accuracy  of  0.001  degrees  at  rates  ranging  from  0.001 
to  80  degrees /second.  In  order  to  mitigate  any  possible 
magnetic  effects  generated  by  the  steel  construction  of  the 
tilt  table,  the  sensor  unit  was  mounted  on  a  non-ferrous 
extension  above  the  table.  In  each  of  these  experiments, 
the  sensor  module  was  initially  placed  with  its  xsyszs  axes 
respectively  aligned  with  the  North-East-Down  directions. 
Following  an  initial  still  period,  the  senor  module  was  then 
subjected  to  a  series  of  rotations. 
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Fig.  4.  Orientation  estimate  produced  by  the  QUEST  and  factored 
quaternion  algorithms  with  a  90°  rotation  in  roll  axis  using  raw 
accelerometer  data. 


Fig.  6.  Orientation  estimate  produced  by  the  QUEST  and  factored 
quaternion  algorithms  with  a  90°  rotation  in  roll  axis  using  low-pass 
filtered  accelerometer  data. 


Figure  4  shows  the  performance  of  the  each  of  the  two 
algorithms  using  raw  accelerometer  and  magnetometer 
data.  The  sensor  was  rotated  -90°  about  the  x-axis  at 
a  rate  of  60°/s  and  then  rotated  90°  at  the  same  rate  (in 
the  reverse  direction)  for  two  cycles.  The  plots  to  the  left 
show  the  orientation  estimated  by  the  QUEST  algorithm, 
and  the  graphs  to  the  right  show  the  orientation  estimated 
by  the  factored  quaternion  algorithm.  The  small  pitch  and 
yaw  motions  seen  in  the  pitch  and  yaw  sub-plots  are  due  to 
misalignment  of  the  sensor  module  with  the  motion  axes 
of  the  tilt  table.  It  can  be  seen  that  both  algorithms  were 
able  to  correctly  estimate  the  roll  angle  before  the  first 
(negative)  rotation,  between  the  first  and  second  (positive) 
rotations,  and  after  the  second  rotation.  Neither  was  able 
to  correctly  estimate  orientation  during  rotational  motion. 
Similar  results  where  observed  in  experiments  involving 
different  angles  of  rotation  at  different  rates. 

During  motion  the  accelerometers  measure  the  sum  of 
gravity  and  motion  induced  acceleration.  In  the  case  of  the 
experiments  described  here,  the  motion  induced  accelera¬ 
tion  is  due  to  the  motion  of  the  tilt  table  and  flexing  of 
the  non-ferrous  extension  on  which  the  sensor  module  was 
mounted.  Since  both  the  QUEST  and  factored  quaternion 
algorithms  are  single- frame  algorithms,  neither  is  able  to 
filter  out  transient  non-gravitational  accelerations  that 
occur  during  motion. 

Figure  5  depicts  a  revised  approach  in  which  a  low- 
pass  filter  for  accelerometer  data  is  combined  with  the 
factored  quaternion  or  QUEST  algorithm.  To  examine  the 
performance  of  the  QUEST  and  factored  quaternion  algo- 
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Magnetometer  Readings 


Fig.  9.  Components  of  the  normalized  local  magnetic  field  measure¬ 
ment  vector  under  the  influence  of  a  moving  magnetic  field  distortion. 


Fig.  7.  Angles,  singularity  condition,  and  switch  flag  of  the  factored 
quaternion  algorithm  during  110°  rotation  in  pitch  axis. 
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Fig.  8.  Components  of  the  estimated  quaternion  produced  by  the  Fig  10  Orientation  estimate  produced  by  the  QUEST  and 
factored  quaternion  algorithm  during  110  rotation  m  pitch  axis.  factored  quaternion  algorithms  with  a  static  sensor  module  under 

the  influence  of  a  moving  magnetic  field  distortion. 


rithms  in  conjunction  with  a  low-pass  filter,  the  rotation 
experiments  were  repeated.  Figure  6  shows  performance 
during  90°  rolls  at  a  rate  of  60°/s.  A  comparison  of  Figure 
6  to  Figure  4  in  which  the  sensor  module  was  subjected 
to  the  same  rotations  shows  that  either  algorithm  can  be 
used  to  track  the  orientation  of  a  rigid-body  in  a  dynamic 
environment  when  acceleration  data  is  low-pass  filtered. 
Again,  similar  results  where  observed  in  experiments 
involving  different  angles  of  rotation  at  different  rates. 

B.  Avoidance  of  Singularity  Conditions 

Within  the  factored  quaternion  algorithm,  three  half 
angles  are  used  to  derive  an  orientation  quaternion. 
Measurement  vectors  are  rotated  by  an  angle  a  when 
the  pitch  angle  approaches  ±90°  and  the  cos  #  approaches 
zero.  Figures  7  and  8  respectively  depict  the  operation  of 
the  factored  quaternion  algorithm  and  its  output  during 
110°  pitch  rotations.  During  this  experiment,  a  was  set  to 
45°  and  e  was  0.2.  The  bottom  two  subplots  of  Figure  7 
trace  the  value  the  cos#  and  the  value  of  the  switch  flag 


that  triggers  the  singularity  avoidance  method  and  show 
the  direct  correspondence  between  the  two  in  time.  It  can 
be  observed  that  each  time  cos#  was  about  to  become 
less  than  e,  the  switch  flag  was  set  to  one.  The  top  three 
subplots  in  Figure  7  depict  the  angles  calculated  from 
the  quaternion  estimate  produced  by  the  algorithm.  The 
apparent  rise  of  the  pitch  angle  to  90°  and  then  drop  to  70° 
is  a  visualization  artifact  due  to  the  use  of  the  three  angles 
for  plotting  purposes.  110°  pitch  is  represented  as  70°  pitch 
together  with  a  180°  roll  and  a  ±180°  yaw.  At  times  the 
yaw  angle  flips  between  two  alternate  representations  of 
the  same  rotation,  namely  -180°  and  180°.  The  roll  angle 
is  stable  at  either  0°  or  180°. 

The  quaternion  elements  depicted  in  Figure  8  are 
smooth  and  exhibit  no  flipping  of  orientation  represen¬ 
tations  or  singularity  artifacts.  The  real  part  of  the 
quaternion,  go?  begins  at  1.0  and  changes  to  cos  Mp-  = 
0.5736  during  the  110°  rotations.  The  element  of  the  unit 
quaternion  associated  with  rotations  about  the  pitch  axis, 
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#2,  begins  at  zero  and  changes  to  sin  =0.8192  during 
the  110°  rotations.  The  small  changes  in  q\  and  q%  are 
due  to  misalignment  between  the  sensor  module  and  the 
motion  axes  of  the  tilt  table. 

C.  Testing  of  Static  Accuracy  When  Subjected  to  Mag¬ 
netic  Field  Variations 

To  test  the  decoupling  property  of  the  factored  quater¬ 
nion  algorithm,  an  inertial/magnetic  sensor  module  was 
mounted  on  a  level  non-ferrous  stationary  platform.  The 
sensor  module  xsyszs-axes  were  respectively  aligned  with 
the  North-East-Down  directions.  Following  an  initializa¬ 
tion  period,  the  sensor  module  was  exposed  to  a  ferrous 
object.  Movement  of  the  ferrous  object  caused  the  di¬ 
rection  of  the  measured  magnetic  field  to  change  by  as 
much  as  360°.  Changes  in  the  measured  magnetic  field 
were  observed  in  all  measurement  axes  as  illustrated  by 
Figure  9.  Figure  10  shows  orientations  calculated  using 
the  QUEST  and  factored  quaternion  algorithms.  It  can 
be  observed  that  the  orientations  calculated  using  the 
QUEST  algorithm  (depicted  by  three  subplots  to  the 
left)  exhibited  errors  on  all  axes.  On  the  other  hand,  the 
factored  quaternion  algorithm  (depicted  by  three  subplots 
to  the  right)  showed  no  errors  in  either  the  pitch  or  roll 
axes. 

D.  Algorithm  Efficiency 

To  make  a  rough  comparison  of  the  efficiency  of  the 
QUEST  and  factored  quaternion  algorithms,  the  time 
required  for  each  to  complete  the  computation  of  5000 
orientation  quaternions  was  determined.  This  number 
represents  50  seconds  of  data  at  a  sampling  rate  of  100 
Hz.  Both  algorithms  were  able  to  complete  the  5000 
quaternion  calculations  in  less  than  10  seconds.  The 
calculations  were  completed  in  9.8  seconds  by  the  QUEST 
algorithm  and  7.8  seconds  by  the  factored  quaternion 
algorithm.  In  this  experiment,  the  factored  quaternion 
algorithm  was  approximately  25%  faster  that  the  QUEST 
algorithm. 

IV.  Conclusion 

The  paper  has  presented  a  intuitive  algorithm  for  calcu¬ 
lating  orientation  using  accelerometer  and  magnetometer 
data.  The  algorithm  produces  estimates  in  quaternion 
form  through  a  series  of  sequential  rotations.  In  the  algo¬ 
rithm,  magnetometer  data  is  not  used  to  calculate  orienta¬ 
tion  relative  to  the  vertical,  thus  magnetic  variations  result 
in  errors  only  in  the  horizontal  plane.  This  property  of  the 
algorithm  is  demonstrated  experimentally.  Singularites  in 
the  numerical  implementation  are  avoided  through  the  use 
of  a  method  that  assigns  an  offset  body  coordinate  system 
when  a  singularity  occurs.  The  algorithm  is  efficient  and 
does  not  require  the  evaluation  of  trigonometric  functions. 
Experimental  results  indicate  that  when  combined  with 
a  low-pass  filter  for  accelerometer  data,  the  algorithm  is 
able  to  track  orientation  of  a  human  limb  segment.  The 
algorithm  has  been  successfully  used  in  real-time  human 
body  motion  tracking  applications. 
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Self-contained  Position  Tracking  of  Human  Movement  Using 
Small  Inertial/Magnetic  Sensor  Modules 

Xiaoping  Yun,  Eric  R.  Bachmann,  Hyatt  Moore  IV,  and  James  Calusdian 


Abstract — Numerous  applications  require  a  self-contained 
personal  navigation  system  that  works  in  indoor  and  outdoor 
environments,  does  not  require  any  infrastructure  support,  and 
is  not  susceptible  to  jamming.  Posture  tracking  with  an  array  of 
inertial/magnetic  sensors  attached  to  individual  human  limb 
segments  has  been  successfully  demonstrated.  The  ’’sourceless” 
nature  of  this  technique  makes  possible  full  body  posture 
tracking  in  an  area  of  unlimited  size  with  no  supporting 
infrastructure.  Such  sensor  modules  contain  three  orthogonally 
mounted  angular  rate  sensors,  three  orthogonal  linear 
accelerometers  and  three  orthogonal  magnetometers.  This 
paper  describes  a  method  for  using  accelerometer  data 
combined  with  orientation  estimates  from  the  same  modules  to 
calculate  position  during  walking  and  running.  The  periodic 
nature  of  these  motions  includes  short  periods  of  zero  foot 
velocity  when  the  foot  is  in  contact  with  the  ground.  This  pattern 
allows  for  precise  drift  error  correction.  Relative  position  is 
calculated  through  double  integration  of  drift  corrected 
accelerometer  data.  Preliminary  experimental  results  for 
various  types  of  motion  including  walking,  side  stepping,  and 
running  document  accuracy  of  distance  and  position  estimates. 

I.  Introduction 

OSITION  tracking  of  human  movement  commonly 
requires  an  unrestricted  line  of  sight  between  one  or  more 
receivers  and  one  of  more  transmitters.  In  inside-out  systems 
a  sensor  attached  to  a  person  to  be  tracked,  passively  or 
actively  receives  information  from  multiple  “sources” 
positioned  around  a  tracking  volume.  In  outside-in  tracking 
systems,  multiple  sensors  positioned  around  a  tracking 
volume  sense  active  or  passive  sources  attached  to  the  object 
to  be  tracked.  The  global  positioning  system  (GPS)  is  a 
familiar  example  of  a  sourced  inside-out  tracking  system. 
Optical  tracking  systems  that  use  multiple  cameras  to  view 
active  or  passive  markers  and  calculate  position  through 
triangulation  are  an  example  of  a  sourced  outside-in  tracking 
system. 

Inside-out  or  outside-in  tracking  systems  require  extensive 
set-up  and  calibration  of  the  tracking  volume.  Line  of  site  and 
noise  restrictions  limit  range  as  well  as  where  these  systems 
can  be  used.  In  some  cases  jamming  or  intentional 
interference  makes  their  use  impractical.  “Sourceless” 
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systems  are  self-contained.  Data  that  are  produced  by  sensors 
attached  to  a  person  can  be  used  to  calculate  position  without 
reference  to  other  devices  or  transmitters.  In  theory,  a 
sourceless  system  with  accuracy  comparable  to  a  sourced 
system  is  superior  since  it  does  not  require  extensive 
infrastructure  positioned  around  or  above  a  tracking 
environment  of  limited  sized  and  is  not  susceptible  to  line  of 
sight  restrictions  between  a  transmitter  and  source. 

Sourceless  orientation  tracking  using  small 
inertial/magnetic  sensor  modules  containing  triads  of 
orthogonally  mounted  accelerometers,  angular  rate  sensors, 
and  magnetometers  has  been  successfully  demonstrated. 
Several  commercial  posture  tracking  systems  based  on 
orientation  tracking  have  resulted.  The  individual  sensors 
used  in  inertial/magnetic  sensor  modules  are  low-cost 
Micro-Electro-Mechanical  Systems  (MEMS)  sensors.  Low 
cost  MEMS  accelerometers  are  susceptible  to  drift  errors. 
Until  recently,  it  was  widely  thought  that  position  tracking 
using  data  from  such  accelerometers  was  not  possible  due  to 
the  quadratic  growth  of  errors  caused  by  sensor  drift  during 
double  integration. 

Most  types  of  human  movement  including  walking,  side 
stepping,  and  running  include  repeated  recognizable  periods 
during  which  the  velocity  and  acceleration  of  the  foot  are 
zero.  These  brief  periods  occur  before  entering  the  swing 
phase  of  the  gait  cycle  each  time  the  foot  contacts  the  ground 
during  the  stance  phase.  Recognition  of  these  periods  allows 
determination  of  the  drift  error  that  occurred  in  between  them. 
This  allows  precise  corrections  to  be  made  to  accelerometer 
data  in  either  a  forward  or  backward  manner.  The  corrected 
accelerometer  data  combined  with  magnetic  and  angular  rate 
data  can  then  be  used  to  calculate  the  direction  and  magnitude 
of  displacement  that  occurs  during  each  step.  This  allows 
accurate  measurement  of  position  relative  to  an  initial  starting 
point. 

This  paper  describes  a  self-contained  method  for  relative 
position  tracking  of  a  human  engaged  in  various  types  of 
motion  involving  discrete  steps.  This  method  is  based  on  the 
use  of  a  single  inertial/magnetic  sensor  module  attached  to  the 
foot.  The  primary  contributions  of  this  work  are: 

•  A  method  for  tracking  2-D  and  3-D  position  of  human 
movement  using  a  self-contained  inertial/magnetic 
sensor  module. 

•  Preliminary  experimental  results  for  various  human 
motion  including  straight  line  walking,  circular  walking, 
side  stepping,  backward  walking,  running,  and  climbing 
stairs. 

This  remainder  of  this  paper  describes  in  detail  how 


accelerometer  data  in  conjunction  with  orientation  estimates 
produced  using  data  from  inertial/magnetic  sensor  modules 
can  be  used  to  track  human  position  in  three  dimensions 
without  any  supporting  infrastructure.  Section  II  presents 
related  work  and  describes  the  foundation  on  which  the  work 
presented  here  is  built.  Section  III  is  a  detailed  description  on 
the  sourceless  position  tracking  method.  Experimental  results 
are  presented  in  section  IV.  The  final  section  is  a  summary 
and  conclusions. 

II.  Background 

Much  research  has  focused  on  using  inertial  and  in  a  few 
cases  magnetic  sensors  to  measure  distance  walked  and/or 
track  position.  Many  methods  have  involved  attempts  to 
count  steps  and  estimate  distance  based  on  an  approximate 
step  length.  Other  work  has  double  integrated  acceleration 
data  recorded  during  the  gait  swing  phase  to  estimate 
distance.  Few  have  attempted  to  determine  the  direction  of 
motion.  In  most  cases,  distance  estimation  errors  when  using 
more  complex  inertial  sensor  combinations  have  been  only 
slightly  better  than  those  obtained  using  commercial 
pedometers. 

Simple  pedometers  focus  on  counting  steps.  Based  on 
this  step  count  and  a  fixed  step  length,  a  pedometer  unit  can 
estimate  distance  traveled.  In  pedometers,  step  count  is 
generally  estimated  by  measuring  vertical  acceleration  using 
a  single  axis  piezo-electric  accelerometer  or  by  monitoring  a 
spring  suspended  horizontal  lever  that  moves  up  and  down  in 
response  to  vertical  accelerations  of  the  hips.  The  accuracies 
of  pedometer  produced  step  counts  vary  greatly  depending  on 
the  type  of  technology  used,  walking  speed,  and  physical 
aspects  of  individuals  begin  tracked  [1].  Pedometers  do  not 
have  the  ability  to  differentiate  between  different  types  of  gait 
such  as  running,  shuffling,  and  side  stepping.  In  [2],  Crouter 
et  al.  tested  and  compared  several  electronic  pedometers  in 
estimating  step  counts  and  distance  traveled  with  subjects 
walking  on  a  treadmill.  Several  models  were  able  to  count 
steps  to  within  ±1%  of  the  actual  value  during  normal 
uniform  walking.  Estimates  of  distance  traveled  were  less 
accurate  with  most  units  estimating  mean  distance  to  within 
±10%  at  a  walking  speed  of  80  meters  per  minute.  Overshoots 
tend  to  occurs  at  slower  speeds.  Undershoots  tend  to  occur  at 
higher  speeds.  In  [3],  Schneider  et  al.  compared  pedometer 
performance  when  subjects  walked  over  a  closed  400  meter 
course.  Accuracy  of  step  counts  as  well  as  distance  estimates 
decreased  in  this  more  natural  environment.  Step  count 
accuracy  decreased  to  ±3%.  Since  walking  speed  and  stride 
length  was  no  longer  artificially  controlled  using  a  treadmill, 
the  accuracy  of  distance  estimates  showed  an  greater 
decrease. 

In  [4],  Pappas  et  al.  describe  a  reliable  gait  phase 
detection  system  based  on  a  single  axis  angular  rate  sensor 
and  three  force  sensitive  resistors.  In  this  system,  all  motion  is 
assumed  to  take  place  in  the  sagittal  plan.  The  angular  rate 
sensor  is  mounted  to  the  heel  with  its  sensing  axis 


perpendicular  to  the  sagittal  plan  and  is  used  to  measure  the 
rotational  velocity  of  the  foot.  The  force  sensitive  resistors  are 
taped  to  the  bottom  of  the  same  foot.  Using  a  heuristic  based 
algorithm  designed  to  detect  four  different  gait  phases 
(stance,  heel-off,  swing,  and  heel- strike),  the  system  was  able 
to  detect  the  phases  with  99%  reliability.  Unlike  simple 
pedometers,  the  described  method  worked  well  to  detect  gait 
phases  during  walking  over  level  and  unleveled  surfaces  as 
well  as  walking  up  and  down  stairs.  In  addition,  the  system 
demonstrated  robustness  in  ignoring  non-gait  events  such  as 
standing  up  and  sitting  down,  bending,  and  turning  in  place. 
The  system  did  not  have  the  ability  to  estimate  distance  or 
direction  traveled. 

Zijlstra  and  Hof  use  a  single  triaxial  accelerometer, 
measured  leg  length,  and  an  algorithm  based  on  an  inverted 
pendulum  model  [5]  to  predict  the  body  center  of  mass 
trajectory  during  walking.  The  method  determines  foot 
contacts  by  monitoring  for  changes  in  sign  of  the  forward 
acceleration  of  the  lower  trunk.  Unlike  pedometers  which  use 
a  fixed  step  length,  mean  step  length  and  walking  speed  are 
estimated  based  on  up  and  down  movement  of  the  trunk. 
Experimental  results  in  [6]  include  data  from  both  treadmill 
and  level  ground  walking  trails.  In  most  cases,  the  described 
method  identified  foot  contacts  with  nearly  100%  accuracy. 
In  treadmill  experiments,  maximum  observed  differences 
between  predicted  speed  and  treadmill  speed  were  no  greater 
than  16%.  In  level  ground  walking  experiments  with 
presumably  less  uniform  gait,  differences  between  predicted 
mean  speed  and  calculated  mean  speed  did  not  exceed  20%. 
This  method  is  able  to  detect  gait  event  with  great  accuracy. 
However,  due  to  the  magnitude  of  the  distance  measurement 
errors  and  the  inability  to  estimate  direction  of  the  travel,  the 
navigation  performance  of  this  method  shows  little 
improvement  over  that  of  a  simple  pedometer. 

Sagawa  et  al.  [7],  Sabatini  et  al.  [8],  and  Cavallo  et  al.  [9] 
use  a  combination  of  accelerometers  and  rate  sensors  attached 
to  the  foot  to  measure  gait  parameters  and  distance  traveled. 
The  Sagawa  approach  uses  a  tri-axial  accelerometer  and  a 
single  axis  angular  rate  sensor  attached  to  the  toe  (an 
atmospheric  pressure  sensor  is  used  to  measure  change  in 
altitude).  The  Sabatini  and  Cavallo  approach  uses  a  bi-axial 
accelerometer  and  a  single  axis  angular  rate  sensor  attached 
to  the  instep. 

Sagawa  et  al.  assumes  that  foot  roll  and  yaw  are  zero 
during  normal  walking.  Sabatini  and  Cavallo  assume  all 
motion  takes  place  in  a  sagittal  plane.  In  both  cases,  a  rate 
sensor  is  mounted  perpendicular  to  the  sagittal  plane.  Gait 
events  such  as  heel-off,  heel-strike,  and  swing  are  detected 
using  angular  rats  data.  Instead  of  counting  steps,  walking 
speed  and  stride  length  are  estimated  by  double  integrating 
acceleration  data  during  the  swing  phase.  For  best 
performance,  the  tracked  subject  is  required  to  maintain  a 
uniform  walking  speed  and  gait.  Both  research  efforts  were 
able  to  detect  gait  events  with  high  levels  of  confidence.  In 
limited  experimental  results,  Sagawa  et  al.  reports  a 


maximum  distance  estimation  error  of  5.3%  over  a  30  meter 
course.  Reported  experimental  results  obtained  while 
walking  over  a  400  meter  closed  course  in  [9]  characterize 
errors  as  being  much  smaller  with  an  average  measured 
distance  of  401.2  ±4.61  meters  or  just  over  a  1%  error. 
Though  GPS  heading  information  was  used  in  [9]  to 
reconstruct  the  path  of  travel,  neither  of  the  systems  described 
is  able  to  determine  the  direction  of  displacement  or  position. 

A  great  deal  of  research  has  focused  on  integrating  inertial 
dead  reckoning  systems  with  positional  information  provided 
by  GPS  and  DGPS.  In  [10],  Jarawimut  et  al.  implement  a 
pedestrian  navigation  system.  During  periods  of  GPS 
availability,  compass  bias  and  average  step  length  are 
updated  to  make  dead  reckoning  results  more  closely  match 
GPS  estimates.  When  GPS  information  is  unavailable, 
distance  traveled  is  calculated  by  multiplying  the  number  of 
steps  times  an  average  step  length.  A  compass  is  used  to 
estimate  the  direction  of  travel  and  the  system  is  able  to 
provide  an  estimate  of  position  as  long  as  the  tracked  subject 
is  walking  in  a  normal  manner. 

Other  attempts  to  produce  a  personal  navigation  based 
on  the  integration  of  inertial/magnetic  sensors  are 
documented  in  [11]  and  [12].  In  [11]  Judd  suggests  that  step 
length  can  be  estimated  based  on  a  linear  relationship  with 
cadence.  The  described  system  consists  of  a  GPS  receiver,  a 
three  dimensional  compass,  and  tri-axial  accelerometer.  The 
accelerometer  is  used  as  a  tilt  sensor  to  determine  the 
horizontal  component  of  the  magnetic  field  and  to  detect  foot 
falls.  Average  step  length  is  estimated  by  a  Kalman  filter 
algorithm.  Distance  traveled  is  based  on  the  product  of  the 
number  of  steps  and  the  estimated  step  length.  Again  this 
approach  is  limited  to  level  walking  in  open  spaces.  The 
personal  navigation  module  described  in  [12]  contains  a 
tri-axial  magnetometer,  a  tri-axial  accelerometer,  a 
barometric  pressure  sensor,  and  a  GPS  receiver.  Distance 
traveled  is  still  based  on  the  step  length/step  count  product.  It 
is  claimed  that  unlike  other  similar  systems,  a  pattern 
recognition  algorithm  is  used  to  identify  acceleration 
signatures  related  to  different  types  of  movement  such  as 
forward  and  backward  walking,  lateral  walking,  and  running. 
Performance  claims  for  a  commercial  version  of  the  system 
give  a  2D  positional  accuracy  of  better  than  5%  of  distance 
traveled  for  “forward  walking  under  normal  conditions  [13].” 
No  accuracy  figures  are  given  for  other  types  of  motion. 
However,  in  independent  use  of  the  product,  the  Sendero 
Groups  reports  typical  errors  on  the  order  of  15%  [14]. 

III.  Method  for  Tracking  Position 

In  theory,  the  output  of  an  accelerometer  can  be 
integrated  twice  to  obtain  displacement  information. 
However,  low-cost  accelerometers  are  susceptible  to  drift 
errors.  The  position  estimates  based  on  double  integration 
can  diverge  in  a  short  time  period  lasting  only  a  few 
seconds.  Drift  correction  is  thus  essential  for  tracking 
position  using  low-cost  accelerometers.  In  this  section,  a 


drift  correction  method  is  first  described.  An  application 
of  this  method  to  position  tracking  of  a  walking  person  is 
then  detailed. 


A.  Correcting  Accelerometer  Drift 

The  drift  correction  method  is  best  illustrated  with  the 
following  experiment.  An  accelerometer  is  first  placed  on 
a  level  table  top,  and  then  is  slid  along  a  straight  line  for  a 
distance  of  one  meter.  The  initial  and  final  velocities  are 
zero.  Figure  1  shows  the  accelerometer  measurement  data, 
as  well  as  estimated  velocity  and  position  for  such  an 
experiment  in  which  an  Analog  Devices  ADXL210E 
accelerometer  was  used.  The  three  plots  on  the  left  side 
show  the  results  of  the  original  data,  and  the  plots  on  the 
right  side  show  the  results  of  the  corrected  data.  The 
correction  procedure  is  discussed  below.  The  velocity  is 
obtained  by  integrating  accelerometer  measurements  once, 
and  the  position  is  obtained  by  integrating  the  velocity  one 
more  time.  While  the  sensor  actually  moved  a  distance  of 
one  meter,  the  estimated  distance  obtained  by  double 
integration  is  0.80m  as  seen  in  the  lower-left  plot.  A  close 
examination  of  the  velocity  in  the  middle-left  plot  indicates 
that  the  final  estimated  velocity  is  -0.23m/s  at  the  end  of  the 
motion  period,  although  the  sensor  stopped  moving  and  the 
actual  velocity  was  zero  at  this  point.  The  error  in  the 
estimated  velocity  is  due  to  drift  in  accelerometer 


Onginal  Data 


Corrected  Data 


Figure  1.  Results  of  a  one-meter  sliding  motion 
experiment  with  original  accelerometer  data  and 
integrated  velocity  and  position  on  the  left  and  the 
drift-corrected  data  and  resulting  velocity  and 
position  on  the  right  side. 

measurements.  Because  the  final  velocity  is  known  to  be 
zero  in  this  case,  a  drift  correction  can  be  applied  to  the 
accelerometer  measurements  so  that  the  final  estimated 
velocity  is  zero.  The  three  plots  on  the  right  side  of  Figure 
1  are  the  corrected  acceleration,  velocity,  and  distance.  It 
is  seen  that  the  final  velocity  is  now  zero.  As  a  result  of  this 
drift  correction,  the  estimated  distance  moved  is  1.01m. 
Clearly,  this  drift  correction  method  makes  it  possible  to 


obtain  accurate  position  information  through  double 
integration.  Many  more  experiments  were  conducted,  and 
similar  results  were  obtained.  Figure  2  shows  the  results 
of  an  experiment  where  the  sensor  was  moved  a  distance  of 
three  meters.  With  the  uncorrected  data,  the  final  estimated 
distance  is  2.01m,  yielding  an  estimation  error  of  33%. 
After  applying  the  drift  correction,  the  final  estimated 
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walking  distance)  of  a  person  is  estimated  and  tracked.  The 
inertial/magnetic  sensor  modules  considered  for  this  study 
contains  triads  of  orthogonally  mounted  accelerometers, 
angular  rate  sensors,  and  magnetometers.  Examples  of  such 
inertial/magnetic  sensor  modules  include  the  MARG  sensor 

[16] ,  the  3DM-GX1  orientation  sensor  from  MicroStrain 

[17] ,  the  nIMU  from  MEMSense  [18],  the  MTx  orientation 
tracker  from  Xsens  [19],  and  the  InertiaCube3  from 
InterSense  [20].  These  inertial/magnetic  sensor  modules 
are  primarily  designed  for  tracking  3 -dimensional 
orientation.  Algorithms  used  by  these  sensor  modules  for 
processing  accelerometer,  angular  rate,  and  magnetometer 
measurements  to  produce  orientation  output  typically  use  a 
Kalman  filter  [21].  In  addition  to  providing  orientation 
output  in  Euler  angles  and/or  quaternions,  some  sensor 
modules  including  the  MARG,  3DM-GX1  and  nIMU  also 
optionally  provide  scaled  measurements  of  acceleration, 
angular  rate,  and  magnetic  field.  3DM-GX1  and  nIMU  are 
used  in  this  study. 

Acceleration  measurements  provided  by  the 
inertial/magnetic  sensor  module  are  in  sensor  or  body 
coordinates.  These  measurements  are  first  transformed  into 
the  earth  coordinates.  The  transformation  is  accomplished 
by  using  the  quaternion  output  of  the  sensor  module.  The 
three  components  of  the  acceleration  measurements  in  the 


Figure  2.  Original  and  drift-corrected  data  for  a 
three-meter  sliding  motion  experiment. 

distance  is  2.99m  with  an  estimation  error  of  only  0.3%. 

B.  Position  Tracking  of  a  Person 

Human  gait  motion  is  cyclic  in  nature.  During  walking, 
each  gait  cycle  consists  of  two  phases:  a  stance  phase  and  a 
swing  phase.  The  stance  phase  is  the  portion  of  the  cycle 
during  which  a  foot  is  in  contact  with  the  ground.  The  swing 
phase  is  the  portion  of  the  cycle  during  which  the  same  foot 
is  not  in  contact  with  the  ground.  The  stance  phase  takes 
approximately  60%  of  the  gait  cycle,  and  the  swing  phase 
takes  the  remaining  40%.  During  walking  (rather  than 
running  or  jumping),  there  are  two  periods  of  time  in  a 
single  gait  cycle  when  two  feet  are  both  in  contact  with  the 
ground.  This  period  of  double  support  occupies  about  20% 
of  the  gait  cycle  [15].  Based  on  the  results  of  experiments 
presented  in  the  previous  subsection,  it  is  possible  to  obtain 
accurate  position  information  by  double  integrating 
accelerometer  measurements  as  long  as  drift  in 
accelerometer  measurements  can  be  corrected.  During  the 
stance  phase,  the  foot  is  in  contact  with  the  ground,  and  foot 
velocity  is  zero.  If  an  inertial/magnetic  sensor  module  is 
attached  to  a  foot,  drift  in  accelerometer  measurements  can 
be  corrected  each  time  the  foot  is  in  the  stance  phase  of  the 
gait  cycle  [7].  If  the  estimated  foot  velocity  is  not  zero,  a 
drift  correction  can  be  applied  to  the  accelerometer 
measurements  as  discussed  in  the  previous  subsection. 
Using  this  approach,  Sagawa,  etc.  [7]  and  Gavallo,  etc.  [9] 
reported  early  efforts  on  estimating  walking  distance. 

In  this  work,  an  inertial/magnetic  sensor  module  is 
attached  to  the  foot,  and  the  3 -dimensional  position  (not  just 


Figure  3.  Three  components  of  the  velocity  obtained  by 
integrating  the  original  acceleration  measurement. 


earth  coordinates  are  then  integrated  to  obtain  velocity 
estimates.  Figure  3  depicts  the  three  components  of  the 
integrated  velocity  for  an  eight-meter  walk.  During  the 
stance  phase,  each  of  the  velocity  components  should  be 
zero.  However,  it  is  seen  that  the  estimated  velocity  tends  to 
drift  over  the  time.  Applying  the  drift  correction  method 
discussed  earlier  each  time  the  gait  cycle  enters  the  stance 
phase  results  in  the  corrected  velocity  profile  shown  in 
Figure  4.  The  corrected  velocity  is  integrated  once  more  to 
obtain  3 -dimensional  position  information.  The  accuracy 
of  the  position  information  will  be  discussed  in  the  next 
section  which  examines  detection  of  gait  events  during 
various  mobility  modes  including  straight  line  walking, 


Figure  4.  Velocity  profile  obtained  from  drift-corrected 
acceleration. 


circular  walking,  running,  side  stepping,  backward 
walking,  and  climbing  stairs. 


C.  Detecting  Gait  Events 


In  order  to  apply  the  drift  correction  method  for 
walking  as  discussed  above,  it  is  necessary  to  reliably 
detect  gait  events,  particularly  the  stance  phase,  using  only 
measurement  data.  Both  accelerometer  and  angular  rate 
data  can  be  used  for  this  purpose. 

Figure  5  shows  the  three  components  of  linear 
acceleration  in  the  earth  coordinates  during  walking. 
While  all  three  acceleration  components  exhibit  a  cyclical 
pattern,  it  can  be  observed  that  z-axis  acceleration  data 
provide  the  strongest  indication  of  gait  events.  During  the 
stance  phase,  acceleration  is  near  zero.  Since  there  are  a 
number  of  zero-crossings  during  the  swing  phase,  a  zero 
threshold  and  a  time  heuristic  must  be  applied  to  the 
acceleration  data  to  detect  stance  phases.  The  time  heuristic 
is  required  to  avoid  classifying  any  zero  crossing  in  the 
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Figure  5.  Three  components  of  the  foot  acceleration  in 
the  earth  coordinate  system. 
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Figure  6.  Foot  angular  rate  in  the  ankle  axis. 

swing  phase  as  a  stance  phase.  If  the  acceleration  is  within 
the  threshold  for  a  specified  period  of  time,  the  foot  is 
determined  to  be  in  the  stance  phase. 

Angular  rate  measurements  also  provide  an  indication 
of  gait  events.  The  angular  rate  in  the  sensor  coordinates 
measuring  ankle  axis  rotation  is  more  prominent  in 
differentiating  the  stance  phase  from  the  swing  phase. 
Figure  6  shows  the  x-axis  (or  ankle  axis)  angular  rate  for  a 
typical  walk.  The  angular  rate  is  near  zero  during  the 
stance  phase.  A  heuristic  similar  to  the  method  discussed 
above  can  be  applied  to  the  angular  rate  data  to  detect  the 
stance  phase.  In  empirical  studies  involving  several 
different  people,  the  use  of  angular  rate  data  was  found  to 
be  more  reliable  than  angular  rate  data. 

IV.  Experimental  Results 

The  following  sub-sections  describe  preliminary 
experimental  results  demonstrating  the  accuracy  of  position 
estimation  using  inertial/magnetic  sensor  modules.  These 
experiments  include  trials  in  which  the  tracked  subject 
walked  a  specified  distance  in  a  straight  line,  walked  around  a 
closed  circuit  that  was  roughly  circular  in  shape,  ran  a 
specified  distance  in  a  straight  line,  and  finally  followed  a 
square  pattern  using  three  different  types  of  motion. 
Preliminary  results  are  also  shown  for  walking  up  stairs.  Data 
for  each  type  of  experiment  was  collected  using  several 
different  individuals.  These  brief  results  are  designed  to 
demonstrate  the  robustness  of  position  tracking  using 
inertial/magnetic  sensor  modules  and  make  apparent  the  wide 
applicability  of  this  method  to  numerous  applications.  At  the 
time  of  this  writing,  further  experiments  are  under  way  as  the 
position  tracking  method  is  further  refined. 

All  experiments  were  conducted  using  a  single  sensor 
module  attached  to  the  foot  as  depicted  in  Figure  7.  Distances 
walked  were  measured  using  a  standard  measuring  tape.  Data 
was  collected  in  real-time  and  post-processed  using  a 
program  written  in  Matlab.  Sampling  rate  was  approximately 
70  Hz. 

A.  Straight  Line  Walking 

Straight  line  walking  experiments  were  conducted  to 
validate  the  feasibility  of  estimating  walking  distance  on  a 
level  surface.  These  experiments  measure  only  displacement 
along  a  straight  line.  No  attempt  was  made  to  estimate 
position.  Table  1  shows  experimental  results  for  24-meter 
straight  line  walk  conducted  in  an  indoor  laboratory 


Figure  7.  MemSense  nlmu  mounted  on  foot  for 
position  tracking  during  walking,  side  stepping, 
and  running. 

environment.  Three  different  experimental  subjects  with 
varying  stride  lengths  were  used.  The  average  distance 
estimation  error  for  the  indoor  walking  experiments  is  5.5% 
with  a  standard  deviation  of  2.4%.  Table  2  shows  results  for 
longer  120-meter  straight  line  walks  conducted  in  an  outdoor 
environment.  Two  different  subjects  were  used  in  these 
experiments.  The  distance  estimation  error  for  this  small 
number  of  experiments  was  less  than  that  observed  during  the 
indoor  experiments  with  an  average  error  of  1.3%  and  a 
standard  deviation  of  1.3%.  Maximum  error  for  the 
120-meter  walking  experiments  was  3.3%. 


Table  1 .  Experimental  results  of  24-meter  straight  line  walk. 


Experiment 

# 

Step  Count 

Estimate 

d 

Distance 

(m) 

Error 

1 

16 

23.59 

1.7% 

2 

16 

21.95 

8.5% 

3 

17 

22.70 

5.4% 

4 

17 

25.61 

6.7% 

5 

17 

25.67 

7.0% 

6 

17 

23.07 

3.9% 

The  marked  difference  in  estimation  accuracy  between 
indoor  and  outdoor  environments  is  attributed  to  errors  in 
transforming  measurement  data  from  senor  coordinates  to  an 
Earth  fixed  coordinate  system.  Magnetometer  measurements 


along  with  accelerometer  and  angular  rate  measurements  are 
used  to  compute  an  orientation  quaternion,  which  is  in  turn 
used  to  transform  data.  In  the  presence  of  magnetic 
interference,  orientation  estimation  algorithms  designed  for 
inertial/magnetic  sensor  modules  exhibit  errors  in  azimuth 
angle  estimates  [22].  In  an  indoor  environment  there  is 
considerably  more  magnetic  interference  due  to  the  presence 
of  file  cabinets,  computers,  monitors,  and  other  laboratory 
equipment.  This  interference  can  causes  estimated  path  of 
travel  to  appear  to  curve  or  wobble  to  the  right  and  left  when 
the  true  path  of  travel  is  a  straight  line.  A  correction  method 
for  these  errors  is  currently  under  investigation. 


Table  2.  Experimental  results  of  120-meter  straight  line  walk 
in  an  outdoor  environment. 


Walker 

Experiment  # 

Step 

Count 

Distance  (m) 

%  Error 

A 

1 

83 

116.03 

3.3% 

A 

2 

82 

119.42 

0.5% 

B 

1 

80 

119.12 

0.7% 

B 

2 

79 

119.05 

0.8% 

B.  Straight  Line  Running 

The  described  position  estimation  method  is  applicable  to 
any  context  involving  repeated  short  periods  during  which 
angular  rate  and  velocity  are  zero.  During  running,  as  with 
walking,  there  are  brief  periods  of  time  in  the  gait  cycle 
during  which  the  foot  is  in  contact  with  the  ground.  Although 
these  zero  velocity  periods  are  relatively  short,  the  same 
method  can  be  used  to  correct  drift  in  accelerometer 
measurements.  Relative  to  walking,  it  is  more  difficult  to 
detect  the  stance  phase  from  running  data  due  to  the  short 
duration  of  these  periods. 

Straight  line  running  experiments  were  conducted  over  the 
same  120-meter  course  used  in  the  outdoor  walking 
experiments.  Again  these  experiments  tested  only  the  ability 
to  measure  displacement  along  a  straight  line.  Table  3  shows 
the  results  of  two  running  experiments  over  a  120-meter  long 
course.  The  maximum  error  for  these  experiments  was  within 
4.75%  of  the  actual  distance  covered. 


Table  3.  Experimental  results  of  120-meter  straight  line 
running 


Test 

# 

Step 

Count 

Actual 
Distance  (m) 

Estimated 
Distance  (m) 

Error 

1 

57 

120.0 

115.4 

3.80% 

2 

54 

120.0 

114.3 

4.75% 

C.  Circular  Walking 

Circular  or  curved  walking  experiments  were  the  first  to  be 
conducted  in  order  to  validate  the  feasibility  of  tracking  2-D 
position.  During  these  experiments  the  position  of  foot  was 
simultaneously  monitored  by  an  optical  tracking  system. 
Figure  8  shows  the  position  as  estimated  using 
inertial/magnetic  sensor  module  data.  Both  axes  are  plotted  in 
meters.  The  starting  and  ending  point  for  the  foot  was  the 
same  point.  This  point  is  (0,  0)  in  the  plot.  Although  truth 
reference  data  is  not  available  as  of  the  time  of  this  writing, 
the  accuracy  of  2-D  position  tracking  can  be  seen  by 
observing  that  the  estimated  trajectory  returns  to  the  starting 
point  following  the  period  during  which  the  walk  occurred 
with  high  accuracy. 


Figure  8.  Position  tracking  of  circular  walking 
trajectory. 


D.  Combined  Forward  Walking,  Side  Stepping,  and 
Backward  Walking 

To  demonstrate  that  the  position  tracking  method  can  be 
applied  to  mixed  types  of  human  movement,  a  5.5  meter 
square  pattern  was  measured  and  marked  in  an  outdoor 
environment.  The  test  subject  followed  this  marked  course 
by  walking  forward  on  the  first  leg  of  the  square,  side 
stepping  to  the  right  on  the  second  leg  of  the  square,  walking 
backward  on  the  third  leg  of  the  square,  and  side  stepping  to 
the  left  on  the  last  leg  of  the  square  before  the  foot  was 
returned  to  the  starting  point.  Figure  9  shows  the  position 
tracking  results  for  this  mixed  motion  experiment.  The  x-axis 
is  the  north  direction,  and  the  y-axis  is  in  the  east  direction. 
The  starting  and  ending  point  is  again  (0,  0).  It  can  be 
observed  that  the  end  point  and  starting  point  almost  coincide, 
with  a  separating  distance  of  0.08  meters.  The  estimated  total 
walking  distance  is  21.6  meters,  while  the  actual  total 
distance  is  22.0  meters  giving  a  distance  estimation  error  of 
1.8%. 

E.  Climbing  Stairs 

The  inertial/magnetic  sensor  module  provides 


3 -dimensional  acceleration  measurements  in  x-,  y-,  and 
z-axes.  Thus,  it  is  possible  to  track  3-dimensional  position. 
The  experiments  described  so  far  were  primarily  concerned 
with  correcting  and  integrating  x-  and  y-axis  acceleration. 
Vertical  axis  acceleration  can  be  corrected  and  integrated  in 
the  same  manner  in  order  to  estimate  relative  height.  Figure 
10  depicts  the  3-D  estimated  trajectory  of  a  person  who 
climbed  stairs  shown  in  Figure  11.  In  can  be  qualitatively 
observed  that  the  estimated  trajectory  in  Figure  10  closely 
resembles  the  actual  profile  of  stairs. 


Figure  9.  Position  tracking  results  of  combined 
forward  walking,  side  stepping,  and  backward 
walking. 


Figure  10.  Estimated  3-D  position  of  a  person 
climbing  stairs  shown  in  the  next  figure. 

V.  Work  In  Progress 

At  the  time  of  this  writing  further  experiments  are  being 
conducted  to  evaluate,  improve,  and  document  the  accuracy 
of  position  estimation  using  inertial/magnetic  sensor 
modules.  These  experiments  include  mixed  motion  types  and 
additional  tracking  methods  for  the  purpose  of  providing 
truth  data. 


The  experimental  results  provided  in  this  paper  were 
obtained  by  post-processing  the  sensor  data.  Efforts  are 
currently  underway  to  implement  a  real-time  system.  This 
system  will  be  integrated  into  an  immersive  virtual 
simulation. 

As  seen  in  the  indoor  walking  experiments,  orientation 
estimation  errors  caused  by  a  non-uniform  magnetic 
environment  can  cause  errors  in  transforming  data  from 
sensor  coordinates  to  Earth  coordinates.  A  correction  method 
has  been  devised  and  is  currently  being  tested. 


Figure  11.  Photo  of  the  stairs  used  in  the  experiment 
for  estimating  3-D  position. 


VI.  Conclusion 

Self-contained  position  tracking  using  data  from 
inertial/magnetic  modules  has  applicability  to  a  wide  number 
of  applications.  Preliminary  experimental  results  presented  in 
this  paper  document  that  this  technique  can  be  used  to  track 
three  dimensional  position  during  a  variety  of  motion  types. 
Estimated  errors  from  these  experiments  indicate  that  the 
method  is  accurate.  Work  is  currently  underway  to  further 
refine  the  method. 
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Abstract  -  A  human  body  motion  tracking  system  based  on 
use  of  the  MARG  (Magnetic,  Angular  Rate,  and  Gravity) 
sensors  has  been  under  development  at  the  Naval  Postgraduate 
School  and  Miami  University.  The  design  of  a  quaternion- 
based  Kalman  filter  for  processing  the  MARG  sensor  data  was 
described  in  [1].  This  paper  presents  the  real-time 
implementation  and  testing  results  of  the  quaternion-based 
Kalman  filter.  Experimental  results  validate  the  Kalman  filter 
design,  and  show  the  feasibility  of  the  MARG  sensors  for  real¬ 
time  human  body  motion  tracking. 

Index  Terms  -  Quaternion-based  Kalman  filter ,  human 
body  motion  tracking,  MARG  sensors,  inertial/magnetic  sensors. 

I.  Introduction 

Inertial/magnetic  sensor  modules  can  be  used  to 
estimate  orientation  of  a  rigid  body  relative  to  an  Earth  fixed 
reference  frame  without  the  need  of  an  artificially  generated 
reference.  The  estimates  produced  are  based  entirely  on 
inertial  quantities  related  to  the  motion  and  attitude  of  the 
module  and  the  orientation  of  the  ambient  magnetic  field 
relative  to  the  module.  If  a  single  sensor  module  is  placed 
on  each  of  the  segments  of  an  articulated  rigid  body,  the 
“posture”  of  the  structure  can  be  determined.  Such 
“sourceless”  orientation  tracking  has  significant  advantages 
over  other  methods  owing  to  its  low  susceptibility  to  various 
sources  of  noise  and  lack  of  range  limitations  [8,9].  If  the 
human  body  is  modeled  as  articulated  rigid  bodies 
consisting  of  approximately  fifteen  segments,  posture  and 
gait  could  be  accurately  tracked  and  measured  over  an 
unlimited  area.  Thus,  this  methodology  of  body  tracking 
could  have  important  applications  in  virtual  environments, 
robotic  teleoperation,  personal  navigation,  and  human 
monitoring  applications  [10]. 

The  Naval  Postgraduate  School  and  Miami  University 
have  teamed  up  to  develop  an  inertial/magnetic  sensor 
module  called  the  MARG  sensor  for  tracking  human  body 
motions  in  real  time  [2].  MARG  (Magnetic,  Angular  Rate, 
and  Gravity)  sensor  modules  contain  three  magnetometers, 
three  angular  rate  sensors,  and  three  accelerometers.  Each 
sensor  type  is  orthogonally  mounted  in  a  triad.  This  paper 
presents  the  implementation  and  experimental  testing  results 
for  a  quaternion-based  Kalman  filter  designed  for  the 
MARG  sensors. 

An  earlier  version  of  the  Kalman  filter  implemented 
here  was  described  in  [1].  The  overall  filter  design  remains 
unchanged.  However,  some  portions  of  the  filter  design 


have  been  modified.  In  particular,  the  original  design  used  a 
reduced-order  Gauss-Newton  method  to  compute  an 
orientation  quaternion  from  accelerometer  and 
magnetometer  measurements.  This  part  of  the  filter  was 
first  modified  to  use  the  QUEST  Algorithm  [3]  and  later  the 
Factored  Quaternion  Algorithm  [4,5].  The  QUEST 
algorithm  [3,6]  was  created  to  determine  the  attitude  of  a 
rigid  body  in  reference  to  a  fixed  coordinate  system,  using  a 
set  of  measurement  vectors.  The  algorithm  computes  a 
rotation  (attitude)  quaternion  that  rotates  the  measurement 
vectors  to  match  the  reference  vectors.  More  recently,  the 
Factored  Quaternion  Algorithm  [4]  was  derived.  It  has  the 
same  goal  as  the  QUEST  algorithm  but  orientation  estimate 
are  derived  through  the  measurement  of  sequential  rotations 
about  three  orthogonal  axes.  It  has  been  shown  that  the 
Factored  Quaternion  Algorithm  has  equal  or  better 
performance  than  the  QUEST  algorithm  in  estimating 
orientation  quaternions  with  MARG  sensor  measurements 
[5].  Nevertheless,  the  Factored  Quaternion  Algorithm  is 
computationally  more  efficient  by  about  25%,  and  is  thus 
used  as  part  of  the  filter  design  in  the  latest  implementation 
[5], 

This  paper  is  organized  as  follows.  Section  II  presents 
the  process  model  of  the  Kalman  filter  for  human  body 
motion  tracking.  Section  III  describes  implementation 
issues  of  the  Kalman  filter  with  a  focus  on  how  the 
nonlinear  process  model  was  first  linearized  and  then 
discretized.  Experimental  modeling  of  the  process  noise 
covariance  matrix  and  the  measurement  noise  covariance 
matrix  is  also  detailed.  Section  IV  reports  the  MATLAB 
simulation  and  offline  testing  results  of  the  Kalman  filter. 
Section  V  describes  the  implementation  and  testing  results, 
followed  by  conclusions  in  section  VI. 

II.  Kalman  filter  process  model 

The  process  model  of  the  quaternion-based  Kalman 
filter  presented  in  [1]  will  be  briefly  reviewed  in  this 
section.  A  diagram  of  the  process  model  is  shown  in  Figure 
1.  In  this  model,  the  angular  rates  co  in  body  coordinates 
are  assumed  to  be  generated  by  a  first-order  linear  system 
with  a  white  noise  forcing  function  w.  The  time  constant  of 
the  first-order  linear  system  is  t  .  The  orientation  estimate 
produced  by  the  filter  is  q  .  The  angular  rates  co  and  the 
quaternion  derivative  q  are  related  by  [7] : 
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Figure  1.  Kalman  Filter  Process  Model. 


q  =  —  q  ®co 


(1) 


where  q  is  the  orientation  quaternion  in  Earth  coordinates, 
and  ®  represents  quaternion  multiplication.  In  order  to 
take  advantage  of  computational  simplifications  and 
efficiencies  possible  of  unit  quaternions,  the  quaternion  is 
normalized  to  unit  length  in  the  last  step  of  the  process 
model.  It  is  noted  that  quaternions  are  used  to  represent 
orientation  in  the  filter  design  because  quaternions  do  not 
have  the  singularity  problem  associated  with  Euler  angles 
and  eliminate  the  computational  expenses  related  to 
approximation  of  transcendental  functions. 


The  state  vector  is  defined  as  a  7-dimensional  vector 
with  the  first  three  components  being  the  angular  rates  and 
the  last  four  being  the  elements  of  the  quaternion.  The 
process  model  expressed  in  terms  of  state  equations  is 
characterized  as  follows: 


x.  = — x.  +  —  w.  (f)  i  =  1,2,3  (2) 

Ti  Ti 

for  the  angular  rates,  and 

x4  —  \xjX5  +  x2x6  +  x3x7  ] ,  (3) 

x5=^[x,x4-x2x7+x3x6],  (4) 

x6  =  -J  [x,x7  +  x2x4  -  x3x5  ] ,  (5) 

x7=~2  \-XjX6  +  +  x3x4  ]  (6) 


for  the  quaternion  components. 

The  MARG  sensor  provides  a  9-dimensional 
measurement  vector,  consisting  of  three  elements  of  the 
linear  acceleration  vector,  three  elements  of  the  local 
magnetic  field,  and  three  elements  of  the  angular  rate 
vector.  If  this  nine-dimensional  measurement  vector  is 
provided  directly  to  the  Kalman  filter  as  measurements,  the 
measurement  equations  are  nonlinear  and  the  resulting 
Kalman  filter  becomes  complex  and  computationally 
expensive.  An  alternative  approach  to  the  Kalman  filter 
design  was  suggested  in  [1].  This  approach  uses  the 
Newton  method  or  a  reduced-order  Gauss-Newton  method 
to  find  a  quaternion  corresponding  to  each  set  of  accelerator 
and  magnetometer  measurements.  These  computed 
quaternion  and  angular  rate  measurements  are  then 
presented  to  the  Kalman  filter  as  measurements.  As  a  result, 


the  measurement  equation  for  the  Kalman  filter  is  linear  and 
is  given  by: 

z  =  Hx  +  v(t )  (7) 

where  z  is  the  seven-dimensional  measurement  vector,  H  is 
a  7  x  7  identity  matrix,  and  v  is  the  vector  of  measurement 
noises. 

Although  the  reduced-order  Gauss-Newton  method 
presented  in  [1]  was  considerably  more  efficient  than  the 
full-order  Gauss-Newton  method,  it  still  is  an  iterative 
method  that  needs  to  be  executed  several  times  before 
convergence  occurs.  Following  additional  work,  the 
reduced-order  Gauss-Newton  method  was  replaced  by  the 
QUEST  Algorithm  [3,6],  and  more  recently  by  the  Factored 
Quaternion  Algorithm  [4].  Both  the  QUEST  Algorithm  and 
Factored  Quaternion  Algorithm  take  a  set  of  the 
accelerometer  and  magnetometer  measurements  and 
produce  an  orientation  quaternion.  They  are  appropriate  for 
orientation  estimation  in  static  or  slow  moving  applications 
where  linear  acceleration  does  not  comprise  a  significant 
part  of  the  total  acceleration  measurements.  The  Factored 
Quaternion  Algorithm  is  computationally  about  25%  more 
efficient  than  the  QUEST  Algorithm. 

III.  Kalman  filter  implementation 

In  this  section,  the  implementation  of  the  Kalman  filter 
based  on  the  process  model  presented  in  the  previous 
section  will  be  described.  It  is  noted  that  although  Equation 
(2)  is  linear,  Equations  (3)  to  (6)  are  nonlinear.  As  a  result, 
an  extended  Kalman  filter  must  be  used.  Additionally,  these 
continuous  equations  must  be  discretized  for  digital 
implementations. 

A.  Discrete  Extended  Kalman  Filter 

Equations  (2)  to  (6)  can  be  written  in  vector  form  as 
follows: 

x  =  f(x)  +  w(t).  (8) 

This  nonlinear  state  equation  is  linearized  along  the 
currently  estimated  trajectory  x  : 

A x  +  w(t),  (9) 

where  the  actual  trajectory,  x ,  is  the  sum  of  estimated 
trajectory  x  and  the  small  increment  Ax 


x  =  x  +  Ax.  (10) 

Equation  (9)  is  linear,  but  it  is  still  in  the  continuous  time 
domain.  The  next  step  is  to  discretize  it  to  obtain  a  discrete 


Figure  2.  Diagram  of  the  extended  Kalman  filter. 

time  process  model.  Let  At  be  the  sampling  interval.  Then 
the  difference  equation  corresponding  to  the  differential 
equation  (9)  is  given  by: 

Ax(tM)  =  <S>kAx(tk)  +  w(tk)  (11) 

where  the  discrete  state  transition  matrix  is: 
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and  the  elements  of  discrete  white  noises  are  given  by: 

f  tk+1  jtk+rv) 


W, 


J  e  T‘  wi(y)dy 

h 


i  =  1,2,3 


(12) 


0  i  =  4,5, 6,7 


Equation  (7)  is  linear.  Thus,  linearization  is  not  needed. 
The  corresponding  discrete  process  model  equation  is 
simply  given  as: 


Hkxk+v, 


(13) 

A  standard  discrete  Kalman  filter  may  now  be  designed 
for  the  discrete  process  equation  (11)  and  the  discrete 


measurement  equation  (13).  A  complete  diagram  of  the 
extended  Kalman  filter  is  depicted  in  Figure  2. 

B.  Modeling  of  Process  and  Measurement  Noises 

In  order  to  implement  the  Kalman  filter  described 
above,  it  is  necessary  to  determine  values  of  the  process 
noise  covariance  matrix  Q  and  the  measurement  noise 

k 

covariance  matrix  Rk  .  These  matrices  represent  the 

confidence  in  the  system  model  and  the  measurement  data, 
respectively. 

The  process  noise  matrix  Qk  is  given  by: 

Qt=E[w(tt)w(tky]  (14) 


where  E  is  the  expectation  operator,  and  w(tk)  is  the 
discrete  white  noise  of  Equation  (12). 

It  is  noted  that  w.  (y)  in  Equation  (12)  is  the 
continuous,  independent  white  noise  process  of  Equations 


(2)  to  (6),  with  zero  mean  and  variance  D  .  Therefore, 


£[w.(?)w.(r)] 


i  =  J 

i  ^  j 


(15) 


This  implies  that  the  process  noise  matrix  is  a  diagonal 
matrix  with  non-zero  elements  only  in  the  first  three 
positions  of  the  main  diagonal,  and  can  be  computed  using 
Equations  (14)  to  (15)  as 
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(16) 


where  qu ,  q„ ,  and  q33  are  given  by: 
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Up  to  this  point,  the  variance  of  the  white  noise 
processes  D  and  the  time  constants  of  the  process  model 

t.  have  been  assumed  known.  To  implement  the  Kalman 
filter,  these  parameters  must  be  determined. 
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Figure  3.  Simulated  angular  rate  (left)  and  actual  angular  rate 
measurements  (right). 


Using  measurement  data  available  from  the  MARG 
sensors,  the  variances  and  time  constants  can  be  found  using 
a  simulated  process  model  for  the  angular  rates,  where  the 
variance  and  time  constants  are  adjusted  until  the  output  of 
the  simulated  model  closely  matched  the  real  data  collected 
from  the  MARG  sensors.  For  this  purpose,  a  sensor  was 
attached  to  the  arm  of  a  person  and  typical  arm  motion  data 
was  collected. 

The  resultant  variances  and  time  constants  are  shown  in 
Table  1,  where  coi ,  oo2  and  co3  are  respectively  the  angular 
rates  about  the  x,  y,  and  z  body  coordinate  axes. 

Table  1.  White  noise  variance  and  the  time  constant 
of  the  linear  system. 


Angular 

Variance  D. 

Time  constant 

rate 

( rad2  j s2 ) 

Ti  (S) 

co1 

50 

0.5 

co2 

50 

0.5 

®3 

50 

0.5 

Figure  3  shows  a  comparison  between  the  simulated 
angular  rates  and  the  actual  angular  rates  obtained  from  a 
MARG  III  sensor  for  typical  arm  motions.  The  graphs  to  the 
left  represent  the  angular  rates  generated  by  the  simulation 
model.  The  graphs  to  the  right  are  the  angular  rates 
measured  by  a  MARG  sensor.  It  can  be  observed  that  the 
two  sets  of  data  exhibit  similar  characteristics. 


The  measurement  noise  covariance  matrix  R 

represents  the  level  of  confidence  placed  in  the  accuracy 
of  the  measurements,  and  is  given  by: 


R=E 

k 


(19) 


Assuming  that  measurements  are  uncorrelated,  Equation 


(19) 

leads 

to 

the 

following 

expression  for  the 

measurement  noise  covariance  matrix: 
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The  diagonal  elements  are  the  variances  of  the 
individual  measurements,  which  can  be  determined 
experimentally  using  measurement  data  from  the  MARG 
sensors.  For  this  purpose,  the  measurements  from  a  static 
MARG  sensor  were  collected.  Table  2  summarizes  the 
values  derived  from  experimental  measurements. 


Table  2.  Elements  of  the  measurement  noise  covariance 
matrix. 
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Table  3.  Convergence  of  the  quaternion  estimate. 
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IV.  MATLAB  IMPLEMENTATION  AND  TESTNG 

After  deriving  all  the  required  parameters  to  initialize 
the  Kalman  filter,  it  was  implemented  using  MATLAB  to 
test  the  performance  and  accuracy  of  the  quaternion 
orientation  estimates  produced  by  the  extended  Kalman 
filter.  Real  world  data  recorded  using  a  MARG  sensor  was 
used  in  these  tests. 

Since  the  Kalman  gain  was  determined  such  that  the 
sum  of  squared  errors  is  minimized,  one  way  to  measure  the 
performance  of  the  Kalman  filter  is  through  examination  of 
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Figure  4.  Trace  of  the  error  covariance  matrix. 
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the  trace  of  the  error  covariance  matrix  Pk .  Figure  4  shows 

the  trace  of  Pk  for  the  first  200  samples  of  data  obtained 

with  the  sensor  in  its  reference  position  (x  axis  pointing 
north,  y  axis  pointing  East,  and  z  axis  point  down).  It  is 
noted,  that  the  sum  of  squared  errors  reaches  a  steady  state 
after  approximately  60  iterations. 

Table  3  shows  the  elements  of  the  quaternion  for  the 
first  five  samples.  The  initial  estimate  was  chosen  to  be  the 
unit  quaternion  (0.5,  0.5,  0.5,  0.5).  The  actual  position  of  the 
sensor  in  the  reference  position  is  represented  by  the 
quaternion  (1,  0,  0,  0).  The  data  shown  in  Table  3  indicates 
that  the  Kalman  filter  estimate  converged  to  the  actual 
position  in  a  single  iteration. 

While  both  the  QUEST  Algorithm  and  the  Factored 
Quaternion  Algorithm  worked  well  for  static  orientation  and 
slow  movements,  the  objective  of  the  Kalman  filter  is  to 
blend  angular  rate  measurements  with  the  estimates 
produced  using  magnetometer  and  accelerometer  data 
during  periods  in  which  the  sensor  module  is  subjected  to 
motions  involving  high  angular  rates  and  large  linear 
accelerations.  To  verify  the  estimation  accuracy  during  such 
periods,  the  orientation  estimates  of  the  Kalman  filter  were 
compared  to  the  estimates  produced  using  only  the  Factored 
Quaternion  Algorithm  with  no  rate  measurement.  Two  kinds 
of  experiments  were  conducted  for  this  test.  The  first  used 
controlled  rotations  produced  by  a  HAAS  precision  tilt 
table.  The  second  used  a  random  motion  pattern  produced 
while  the  sensor  was  attached  to  the  arm  of  a  person. 

In  the  first  set  of  experiments,  the  sensor  was  initially 
placed  at  the  end  of  a  1 -meter  pole  attached  to  the  rotating 
table  with  its  xyz  axes  aligned  with  West-North-Down 
directions.  The  sensor  was  rotated  90°  about  the  y-axis  at  a 
rate  of  60°/s  and  then  rotated  -90°  at  the  same  rate  (in  the 
reverse  direction).  Figure  5  shows  the  performance  of  the 
Kalman  filter  in  estimating  the  orientation  of  the  sensor.  The 
graphs  to  the  left  show  the  orientation  estimated  by  the 
Factored  Quaternion  Algorithm,  and  the  graphs  to  the  right 
show  the  orientation  estimated  by  the  Kalman  filter.  It  can 
be  seen  that  the  Factored  Quaternion  Algorithm  was  able  to 


Figure  5.  Quaternion  estimates  produced  by  the  Factored 
Quaternion  Algorithm  (left)  and  Kalman  filter  (right)  with 
a  90-degree  rotation  in  pitch  axis. 

correctly  estimate  the  pitch  angle  before  the  first  (positive) 
rotation,  between  the  first  and  second  rotations,  and  after  the 
second  (negative)  rotation,  but  it  is  not  able  to  correctly 
estimate  orientation  during  the  rotational  motions.  Large 
errors  in  roll  and  yaw  were  also  produced  by  the  Factored 
Quaternion  Algorithm.  On  the  other  hand,  it  can  be  seen 
from  the  right-center  plot  that  the  Kalman  filter  was  able  to 
correctly  estimate  the  pitch  angle  throughout  the  duration  of 
the  experiment.  The  small  roll  and  yaw  motions  seen  in  the 
top-right  and  bottom-right  plots  are  due  to  misalignment  of 
the  individual  sensor  components  within  the  MARG  sensor 
module. 

Figure  6  shows  the  results  of  an  experiment  in  which 
the  sensor  was  rotated  randomly  while  attached  to  the  arm 
of  a  person.  Although  there  is  no  true  reference  in  this  case, 
it  can  be  seen  that  the  Kalman  filter  eliminated  the  jittering 
and  spiking  contained  in  the  orientation  estimates  produced 
by  using  the  Factored  Quaternion  Algorithm  alone. 

V.  Real-time  Testing  results 

After  initial  testing  of  the  extended  Kalman  filter  with 
the  MATLAB  implementation,  the  Factored  Quaternion 
Algorithm  and  extended  Kalman  filter  algorithm  were 
implemented  in  Java  for  real-time  testing  and  evaluation. 
The  real-time  quaternion  produced  by  the  Kalman  filter  was 
visualized  by  a  human-like  avatar  called  “Andy”  as  seen  in 
Figure  7.  Two  MARG  sensors  were  used  to  track  the 
motion  of  a  human  arm,  one  sensor  being  attached  to  the 
upper  arm  and  the  other  attached  to  the  lower  arm. 

The  Factored  Quaternion  Algorithm  was  able  to  track 
the  motion  of  the  human  arm  under  slow  moving  conditions 
where  linear  acceleration  was  not  significant.  However, 
when  the  arm  motion  became  faster,  the  algorithm  was  not 
able  to  follow  the  arm  motion  resulting  in  observable  lag  as 
well  as  overshoots. 
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Figure  6.  Quaternion  estimates  produced  by  the  Factored 
Quaternion  Algorithm  (left)  and  Kalman  filter  (right)  with 
random  arm  movements. 

When  the  extended  Kalman  filter  was  integrated  with 
the  Factored  Quaternion  Algorithm,  the  avatar  was  able  to 
successfully  track  the  human  arm  motion  in  real  time  under 
all  conditions.  Furthermore,  the  filtering  process  did  not 
produce  any  noticeable  lag.  Movement  of  the  human  arm 
and  the  avatar  was  synchronized. 

V.  CONCLUSIONS 

The  paper  presents  implementation  and  experimental 
results  for  a  quaternion-based  Kalman  filter  designed  for 
real-time  human  body  motion  tracking  using  the  MARG 
sensors.  A  simple  process  model  designed  for  human  body 
motion  tracking  was  first  introduced.  The  model  was  then 
linearized  and  discretized.  Experimental  determination  of 
error  covariance  matrices  was  described.  An  extended 
Kalman  filter  was  implemented,  first  in  MATLAB  for 
offline  evaluation  and  finally  in  Java  for  real-time  testing 
and  evaluation.  The  estimated  orientation  quaternion  was 
visualized  using  a  human  avatar.  Testing  results  indicated 
that  the  Kalman  filter  performed  satisfactorily  for  tracking 
motions  of  a  human  arm  in  real  time  under  all  conditions. 
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Abstract — Real-time  tracking  of  human  body  motion  has 
applications  in  tele-operation,  synthetic  reality  and  others. 
A  motion  tracking  system  based  on  use  of  the  MARG 
sensors  has  been  under  development  at  Naval  Postgraduate 
School  and  Miami  University.  The  Magnetic,  Angular  Rate, 
and  Gravity  (MARG)  sensor  modules  use  a  combination  of 
three  orthogonal  magnetometers,  three  orthogonal  angular 
rate  sensors,  and  three  orthogonal  accelerometers  to 
measure  3-D  orientation  of  individual  limb  segments  in 
order  to  determine  posture.  This  paper  presents  the  latest 
results  of  the  MARG  human  body  motion  tracking  system. 
The  design  and  implementation  of  a  Control  Interface  Unit 
(CIU),  a  real-time  3-D  human  avatar  called  “Andy,”  and  a 
concurrent  client-server  program  are  discussed. 
Experimental  testing  and  evaluation  of  the  overall  MARG 
system  is  also  presented.  The  system  is  able  to  track 
multiple  human  limbs  in  real  time.  The  captured  human 
motion  data  can  be  visualized  over  the  Internet  by  multiple 
clients  using  the  3-D  avatar. 

Keywords  -  Human  body  motion  tracking;  MARG  sensors ; 
avatar;  wireless  communication, 

I.  Introduction 

Accurate  real-time  tracking  of  human  body  motion  is 
important  for  many  applications  that  involve  human- 
machine  interactions.  One  such  application  is  in  virtual 
training  [1].  Real-time  motion  tracking  makes  it  possible 
to  create  immersive  virtual  environments  in  which 
trainees  will  act  and  react  as  if  the  environments  were 
real.  Captured  human  motion  data  can  also  be  used  to 
control  humanoid  robots  [1]  [3].  Measurements  of  human 
body  movements  can  be  used  to  estimate  physical  and 
mental  conditions  of  patients  in  clinical  applications  [4]. 
Motion  tracking  of  human  movements  is  widely  used  in 
sports  training  and  production  of  animated  movies. 

There  are  a  number  of  technologies  for  tracking 
human  body  motion,  including  mechanical  trackers, 
active  magnetic  trackers,  optical  tracking  systems, 
acoustic  tracking  systems,  and  inertial  tracking  systems 
[5]  [6]  [7].  Among  the  inertial  tracking  systems, 
Sakaguchi  et  al.  [8]  describes  a  gyroscope  and 
accelerometer-based  motion  tracking  system  for  tracking 
human  arm  motion.  Lee  and  Ha  [9]  reports  a  study  of 
human  motion  tracking  using  only  accelerometers.  There 
are  broadly  two  kinds  of  image-based  motion  tracking 
methods.  One  method  requires  markers  on  the  tracked 
human  body,  and  other  method  does  not  use  markers. 


OPTOTRAK  from  Northern  Digital  Inc.  is  a  typical 
example  of  a  marker-based  system  [10].  Another  is  the 
motion  tracking  method  developed  for  the  CAVE  system 
[11].  Marker-free  methods  are  in  general  preferred 
because  they  are  less  cumbersome  [12][13].  In  most 
cases,  multiple  cameras  are  used  to  overcome  occlusion 
problems  and  to  construct  3-D  motion  data  from  2-D 
images  [14]  [15]. 

This  paper  presents  a  MARG  sensor-based  motion 
tracking  system.  The  Magnetic,  Angular  Rate,  and 
Gravity  (MARG)  sensor  modules  use  a  combination  of 
magnetometers,  angular  rate  sensors,  and  accelerometers 
to  measure  3-D  angular  motion  of  rigid  bodies.  MARG 
sensors  are  self-contained,  and  do  not  require  any 
artificially  generated  sources.  They  are  constructed  using 
MEMS  sensors.  As  a  result,  they  are  small  and  are 
power-efficient.  MARG  sensor  module  design  and 
implementation  details  were  presented  in  [16].  A 
quaternion-based  Kalman  filter  used  to  process  MARG 
data  was  discussed  in  [17]. 

This  paper  presents  other  components  of  the  MARG 
human  motion  tracking  system,  and  experimental  testing 
results  of  the  overall  system.  These  presented 
components  include  the  Control  Interface  Unit  (CIU),  the 
3-D  human  avatar,  “Andy,”  and  a  client-server  protocol 
for  transmitting  MARG  animation  data.  The  CIU  is 
designed  to  provide  control  signals  to  and  multiplex 
measurement  data  from  multiple  MARG  sensor  modules. 
It  packages  measurement  data  from  up  to  16  MARG 
sensors  for  wireless  transmission  using  the  802.11b 
wireless  LAN  standard.  “Andy,”  the  human  avatar  is  a 
cartoon-type  avatar  developed  using  X3D  [18]  and 
follows  the  H-Anim  specification  [19].  It  is  specialized 
to  allow  animation  using  orientation  data  expressed 
relative  to  an  Earth  fixed  reference  frame  such  as  that 
provided  by  MARG  sensor  modules.  The  MARG  human 
motion  tracking  system  allows  multiple  clients  to 
visualize  the  captured  human  motion  over  the  Internet 
using  the  avatar  Andy,  supported  by  the  client-server 
program. 

II.  MARG  Sensors 

MARG  sensor  modules  are  designed  to  provide  data 
for  measuring  3 -DOF  orientation  in  real  time  without 
singularities  [17].  A  more  detailed  description  of  the 
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design  and  implementation  of  the  third  generation 
prototype  can  be  found  in  [16].  The  dimensions  of  the 
MARG  m  are  28  x  30.5  x  17.3  mm.  It  weighs 
approximately  8.5  grams  (0.3  oz).  Power  consumption  is 
144  mW  (20  mA)  when  powered  with  7.2  Volts. 
Sampling  rate  is  100  Hz.  The  MARG  III  is  fabricated  by 
McKinney  Technology  [20]. 

The  MARG  III  contains  three  major  sensing 
components.  A  pair  of  the  two-axis  (HMC1052)  and  one- 
axis  (HMC1051Z)  magnetic  sensors  are  used  for  low 
frequency,  three  dimensional  measurement  of  the 
direction  of  the  local  magnetic  field  vector.  A  pair  of 
two-axis  Analog  Devices  ADXL202E  acceleration 
sensors  is  used  for  low  frequency,  three  dimensional 
measurement  of  the  gravity  vector  relative  to  the 
coordinate  frame  of  the  sensor  module.  A  triad  of 
orthogonally  mounted  NEC/TOKIN  CG-L43  ceramic 
angular  rate  sensors  are  used  for  high  frequency 
measurement  of  sensor  module  body  rates.  Two  of  the 
three  sensing  components  of  the  MARG  III  (the 
magnetometers  and  the  rate  sensors)  produce  analog  data. 
The  Texas  Instruments  MSP-430F149  microcontroller  is 
a  fourth  major  component  in  the  sensor  module.  It 
performs  the  analog-to-digital  conversion  of  data  and 
transmits  digital  data  to  the  CIU.  After  collection  and 
retransmission  by  the  CIU,  MARG  sensor  data  is 
processed  by  a  filter  that  takes  advantage  of  the 
complementary  characteristics  of  the  installed  sensor 
components  [17]. 


in.  Control  Interface  Unit  (crj) 

A.  Role  of  the  Control  Interface  Unit 

When  completed,  the  MARG  human  motion  tracking 
system  will  deploy  1 5  MARG  sensors  to  track  motion  of 
15  limb  segments.  There  is  a  need  to  multiplex 
measurement  data  from  all  15  MARG  sensors  and 
transmit  them  to  a  network-based  computer  for 
processing.  For  this  and  other  purposes,  the  concept  of 
the  Control  Interface  Unit  (CIU)  was  introduced.  It  is  a 
component  of  the  motion  tracking  system  that  is  designed 
to  be  worn  by  the  user  at  the  waist  or  on  the  back.  All  15 
MARG  sensors  are  connected  to  the  CIU  by  a  custom- 
made  cable.  Through  this  cable,  the  CIU  delivers  the 
power  and  the  clock  signal  to  each  of  the  MARG  sensors. 
The  MARG  sensors  transmit  measurement  data  to  the 
CIU.  The  CRJ  then  multiplexes  the  measurement  data 
from  multiple  MARG  sensors,  and  wirelessly  transmits 
the  data  to  a  networked  PC  (server)  for  processing.  The 
wireless  transmission  is  achieved  using  the  IEEE  802.1  lb 
standard. 

The  MARG  sensor  communicates  with  the  CIU 
through  a  Universal  Synchronous  Asynchronous 
Receiver  Transmitter  (USART)  operating  in  the 
Synchronous  Peripheral  Interface  (SPI)  mode.  In  this 


Figure  1.  The  One-Channel  Control  Interface  Unit 
(One-Channel-CIU). 

configuration,  the  MARG  sensor  operates  as  a  slave 
device  whereas  the  CIU  is  the  master  device.  The  clock 
signal  needed  for  synchronizing  the  data  transmission  is 
delivered  to  the  MARG  sensor  by  the  CIU. 

B.  The  One-channel  CIU 

The  CIU  was  designed  and  implemented  in  stages. 
A  one-channel  CIU  was  designed  and  implemented  first. 
It  connects  to  one  MARG  sensor,  and  delivers  the  output 
data  by  a  standard  RS232  port.  An  802.11b  wireless 
serial  adaptor  named  WiSER2400.IP  from  OTC  Wireless 
Inc.  [21]  was  utilized  for  wireless  transmission  of  the 
output  data  to  the  networked  PC.  A  picture  of  the  one- 
channel  CIU  is  shown  in  Figure  1.  The  main  component 
of  the  one-channel  CIU  is  a  TI  MSP430F149 
microcontroller  identical  to  the  one  onboard  the  MARG 
III  sensor. 


Figure  2.  The  Three-channel  Control  Interface  Unit 
(Three-channel  CIU). 

C.  The  Three-channel  CIU 

After  the  one-channel  CIU  was  designed, 
implemented,  and  successfully  tested,  a  three-channel 
CIU  was  built.  The  puipose  of  the  three-channel  CIU 
was  to  test  motion  tracking  of  multiple  limb  segments 
with  multiple  MARG  sensors,  and  to  test  the  operation  of 


626 


the  human  avatar  Andy  and  the  client-server  program 
(discussed  later).  The  three-channel  CIU  is  shown  in 
Figure  2.  It  is  constructed  from  three  one-channel  CIUs 
in  a  parallel  configuration. 

D.  The  Sixteen-channel  CIV 

The  MARG  motion  tracking  system  is  designed  to 
simultaneously  track  1 5  limb  segments.  For  this  purpose, 
a  sixteen-channel  CIU  was  designed.  Sixteen  rather  than 
fifteen  was  chosen  because  input/output  number  of 
multiplexers  usually  is  in  power  of  two.  This  CIU  is  to 
multiplex  all  measurement  data  from  16  MARG  sensors, 
packages  them  in  a  proper  format,  and  transmit  them 
using  a  single  wireless  communication  channel.  The 
selected  multiplex  method  was  to  use  a  XILINX® 
Spartan™ -II  XC2S100  Field  Programmable  Gate  Array 
(FPGA)  [22].  The  prototype  board  of  the  sixteen -channel 
CIU  is  shown  in  Figure  3. 


Figure  3.  Top  View  of  the  Sixteen-channel  Control 
Interface  Unit  (Sixteen-channel  CIU). 


TABLE  I.  THE  SIXTEEN-CHANNEL  CIU  OUTPUT  FORMAT 


Number  of  Bytes 

Content 

2 

Communication  Synchronization 

2 

MARG  111  “Alive’’  Identification  Sits 

2 

Payload  'Heafth”  Status 

1 

Timing 

1 

Sample  Number 

<13.5  ♦  0.5)  x  16  MARG  HI  sensors 

Payload  and  MARG  W  Identification 

(Total  of  224  bytes) 

Number 

Total:  232  Bytes 

The  data  format  used  by  the  CIU  shown  in  Table  1 
consists  of  232-byte  words,  which  include  the  data  from 
all  sixteen  MARG  ED  sensors  and  the  necessary 
communication  overhead.  In  the  event  that  one  or  more 
MARG  III  sensors  are  not  connected  or  that  they  transmit 
incorrect  data,  the  FPGA  replaces  the  respective  hits  with 
zeros  in  order  to  keep  a  constant  transmission  rate. 

The  data  for  each  of  the  sixteen  MARG  HI  sensors 
consists  of  the  measurements  from  the  three  magnetic 
(Mx,  My,  Mz),  the  three  angular  rate  (Rx,  Ry,  Rz),  and 
the  three  acceleration  sensors  (Ax,  Ay,  Az)  onboard  the 
MARG  HI  sensor  (nine  channels  for  each  sensor).  Each 
channel  (transmitted  in  the  order  of  Rx,  Ry,  Rz,  Ax,  Ay, 


Az,  Mx,  My,  Mz)  occupies  one  and  a  half  byte,  giving  a 
total  of  1 3.5  bytes  of  data  for  each  MARG  in  sensor.  An 
identification  number  of  a  half  a  byte  is  added  to 
associate  the  data  received  with  the  corresponding 
MARG  HI  sensor.  This  ID  number  leads  to  a  total 
payload  of  14  bytes  for  each  MARG  El  sensor. 

An  802.11b  wireless  LAN  OEM  module  Airborne 
from  DP  AC  Technologies  [23]  was  used  for  wireless 
transmission.  The  Airborne  unit  is  interfaced  to  the  TI 
microprocessor  onboard  the  CIU  using  UART.  The  data 
transmission  rate  is  232  kbps. 

IV.  Human  Avatar  Andy 

Avatar  Andy  was  developed  to  allow  networked 
viewing  of  human  body  motion  using  a  web  browser.  It 
is  a  cartoon-type  avatar  created  using  the  Extended  3D 
(X3D)  language  [18].  It  is  a  modification  of  the  low- 
resolution  avatar  named  AndyLow,  developed  by 
Seamless  Solutions,  Inc.  [24].  AndyLow  was  originally 
implemented  using  the  Virtual  Reality  Modeling 
Language  (VRML)  and  follows  the  H-Anim  specification 
[19].  It  was  converted  to  X3D,  an  extended  version  of 
VRML  [25].  Modification  of  AndyLow  was  required  due 
to  the  characteristics  of  orientation  estimates  produced 
from  MARG  data  [25]. 

The  geometries  of  all  limb  segments  in  the  AndyLow 
avatar  are  described  relative  to  a  single  unique  reference 
frame,  located  at  the  center  point  between  the  feet.  Limb 
segments  are  arranged  in  a  hierarchy  radiating  from  this 
reference  point  with  the  segments  that  are  closer  to  the 
reference  being  termed  “inboard”  of  those  that  are  further 
away.  Joint  rotations  for  each  limb  segment  must  be  set 
using  an  orientation  that  is  relative  to  the  reference 
frames  of  each  of  the  more  inboard  joints.  Limb  segment 
orientation  estimates  derived  from  MARG  sensor  data 
are  given  relative  to  an  Earth  fixed  reference  frame.  This 
requires  that  each  limb  segment  be  oriented 
independently  of  all  other  segments.  For  this  reason, 
AndyLow  was  incompatible  with  the  system  described 
here.  To  overcome  this  drawback,  each  segment’s 
geometry  was  redefined  using  its  own  local  reference 
position  with  only  a  connection  point  to  the  parent  or 
next  most  inboard  segment. 

The  H-Anim  specification  [  1 9]  defines  several  levels 
of  articulation.  Highly  detailed  levels  allow  for  the 
individual  animation  of  minor  limb  segments  such  as 
finger  joints.  Coarser  levels  only  allow  individual 
animation  of  major  limb  segments  such  as  the  upper  leg 
or  lower  arm.  Level  one  articulation  (LOA-1)  is  preferred 
for  AndyLow.  This  level  offers  18  joints  arranged  in  a 
hierarchical  human  skeleton  structure.  The  MARG 
system  was  designed  to  track  up  to  15  individual  limb 
segments.  Avatar  Andy  fixes  the  sacroiliac ,  Imidtarsal 
and  rjnidtarsal  joints  in  order  to  reduce  the  total  number 
limb  segments  to  15.  Figure  4  shows  the  skeleton 
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structure  of  the  avatar  Andy  with  numbering  used  to 
represent  the  joints.  Figure  5  shows  Andy  in  a  standing 
position. 


Humane  idRoot :  sacrum/pelvis 

10} 

|  l_hip  :  l_thigh  * 

[1] 

|  1  knee  :  1  calf 

[21 

j  1  ankle  :  I  hindfoot 

Dl 

|  r  hip  :  r  thigh 

[41 

[  r  knee  :  r  calf 

[S3 

1  r  ankle  :  r  hindfoot 

I*] 

}vl5  : 15 

V) 

|  skullbase  :  skull 

[81 

|  l_shoulder :  l_upperarm 

[9] 

|  l_elbow  :  l_forearm 

[10] 

|  I  wiist :  l  hand 

[11] 

|  r_shoulder :  r_upperaim 

[12] 

|  r_elbow :  r_forearm 

[13) 

[  rwrist :  r_hand 

[14] 

Figure  4.  Hierarchical  Skeleton  Structure  of  the  Avatar 
Andy. 

Real-time  orientation  data  is  supplied  to  avatar  Andy 
through  a  Java  Script  node.  The  node  contains  a  TCP 
socket  for  handling  control  information  and  commands. 
Orientation  data  is  received  via  a  separate  UDP  socket. 
The  avatar  is  thus  capable  of  controlling  all  its  15  joints 
in  networked  virtual  environments  (NVES). 


V.  Concurrent  Client-server  Protocol 


Networking  capability  was  added  to  the  MARG 
system  in  order  to  produce  a  flexible  system  with  real¬ 
time  data  streaming.  Therefore,  a  concurrent  client-server 


program  is  developed  to  provide  a  network  interface  to 
the  system.  The  concurrent  client-server  program 
receives  MARG  sensor  data  through  a  UDP  socket  and 
delivers  the  data  to  the  clients  on  the  wide  area  network 
(WAN)  simultaneously.  Delivering  the  same  motion  data 
to  multiple  clients  simultaneously  is  implemented  by  a 
method  called  Multicasting  Using  TCP  and  UDP 
Protocol  (MUTUP)  [25]. 

Multicasting  is  the  most  efficient  way  of  transmitting 
information  among  a  large  number  of  group  members 
spread  out  over  different  networks.  Reduced  network 
bandwidth  use  is  the  major  advantage  of  using 
multicasting  protocols.  Unfortunately,  most  routers  on 
the  Internet  are  not  configured  for  multicasting.  A 
technique  called  tunneling  is  used  to  overcome  this 
problem.  Tunneling  is  a  software  solution  that  runs  on 
the  end  point  routers/computers  and  allows  multicast 
packets  to  traverse  the  network  by  putting  them  into 
unicast  packets.  MUTUP  overcomes  the  tunneling 
problem  using  shared  memory  in  the  server  and  a  unicast 
TCP  and  UDP  messages  between  the  server  and  each 
client.  The  major  disadvantage  of  MUTUP  is  a  limitation 
on  the  number  of  clients  that  can  be  handled  by  the  server 
at  any  time.  This  limitation  is  caused  by  an  increase  in 
load  on  the  CPU  and  additional  memory  consumption  for 
each  client.  Low  performance  or  out-of-memoiy 
problems  may  occur  if  the  server  must  handle  too  many 
clients.  MUTUP  also  uses  greater  network  bandwidth 
than  multicasting  because  separate  update  messages  must 
be  sent  to  each  client.  Since  a  relatively  small  number  of 
clients  are  expected  in  the  MARG  project,  MUTUP  was 
chosen  as  an  alternative  method  to  the  multicasting 
protocol  despite  its  drawbacks. 

MUTUP  uses  shared  memory  in  the  server  program 
for  storing  the  latest  motion  data.  Clients  request  TCP 
connections  from  the  server.  The  server  accepts  the 
requests  and  creates  a  separate  thread  for  handling  each 
of  the  connections.  The  TCP  connection  is  used  for 
general-purpose  communication.  The  TCP  protocol  is  not 
appropriate  for  data  streaming  due  to  increased  latency 
and  overhead.  Therefore,  a  second  connection  based  on 
UDP  sockets  is  established  between  the  client  and  the 
server.  The  server  program  asks  the  client  to  create  a 
UDP  socket  and  send  the  IP  address  and  the  UDP  port 
number  of  this  socket  back  to  the  server.  The  server  adds 
the  IP  address  and  the  UDP  port  number  of  the  client  as  a 
destination  for  the  packets  sent  by  the  server  program. 
To  provide  the  same  motion  data  for  all  connected  clients 
simultaneously,  a  shared  memory  array  that  always  stores 
the  latest  update  is  created  on  the  server  program.  An 
updater  thread  updates  this  array.  All  client  handler 
threads  access  this  array  at  any  time  they  want.  A 
diagram  of  MUTUP  is  provided  in  Figure  6. 
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VI.  TESTING  and  Evaluation 

The  performance  of  the  individual  MARG  sensors 
was  tested  first.  Each  MARG  sensor  produces  nine 
components  of  raw  measurement  data  at  the  rate  of  100 
Hz.  The  raw  measurement  data  are  processed  by  the 
filter  algorithm  [17]  to  produce  a  quaternion 
representation  of  orientation.  For  plotting  purposes, 
quaternions  are  converted  to  Euler  angles  (roll  <p,  pitch  0 
and  yaw  y).  Figure  7  shows  the  output  of  the  MARG 
sensor  as  it  performs  a  720°  rotation  (roll  (p)  about  its 
longitudinal  axis.  It  is  seen  that  the  sensor  starts  from  a 
zero  roll.  When  the  motion  starts,  the  sensor  responds 
with  a  linear  rotation  toward  negative  180°.  The  plot 
displays  angles  within  the  range  of  negative  180°  and 
positive  180°.  Therefore,  a  sudden  transition  from 
negative  180°  to  positive  180°  appears.  In  reality,  the 


sensor  continues  to  rotate  with  a  constant  angular  rate 
until  it  reaches  a  full  720°  rotation  and  then  stops.  A 
slight  pitch  deviation  and  an  even  smaller  yaw  deviation 
are  also  observed. 
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Figure  7.  Response  of  MARG  Sensor  to  720°  Roll  Motion. 

The  real-time  performance  of  the  overall  tracking 
system  was  evaluated  in  connection  with  the  human 
avatar  Andy.  Two  MARG  sensors  were  attached  to  an 
arm  of  a  user,  and  connected  to  the  three-channel  CIU. 
The  CIU  then  wirelessly  transmitted  the  sensor  data  to  a 
network-based  server  computer.  The  sensor  data  were 
filtered,  and  the  resulting  orientation  quaternions  were 
transformed  into  axis-angle  pairs.  This  transformation 
was  necessary  since  the  avatar  was  created  using  the 
X3D  language,  which  has  been  standardized  to  use  axis- 
angle  pairs  to  represent  rotations. 

Testing  results  with  the  full-body  avatar  were  very 
successful.  With  the  use  of  two  MARG  in  sensors,  the 
avatar  followed  the  motion  of  the  human  right  arm 
exactly.  Figure  8  and  Figure  9  show  two  snapshots  of  the 
testing  scene.  The  user  moves  his  arm,  and  the  motion  is 
followed  in  real  time  by  the  avatar. 

VH.  CONCLUSION 

The  paper  presents  the  system  components  of  a  body 
motion  tracking  system  based  on  MARG  sensor  modules 
and  testing  and  evaluation  results  for  a  prototype  three¬ 
sensor  system.  The  components  presented  include  a 
Control  Interface  Unit,  a  human  avatar,  and  a 
Client/Server  protocol  for  transmitting  animation  data. 
The  CIU  packages  data  from  up  to  16  MARG  sensors  for 
wireless  transmission.  The  Client/Server  program 
receives  MARG  sensor  data  and  delivers  that  data  to 
multiple  clients  simultaneously.  The  avatar  allows 
networked  viewing  of  animations  produced  using  MARG 
data  in  real  time.  Tests  of  a  prototype  three  sensor  system 
indicate  that  these  components  provide  the  necessary 
infrastructure  to  support  a  1 6  sensor  system  for  full  body 
tracking. 
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Figure  8.  Avatar  Andy  and  the  User  in  Real-Time 
Testing. 


Figure  9.  Another  View  of  Avatar  Andy  and  the  User  in 
Real-Time  Testing. 
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